#include <utils.h>
#include <cmath>
#include <vector>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <boost/lexical_cast.hpp>
#include <iomanip>
#include <ros/package.h>

#define DEG2RAD 0.01745329
palette GetPalette(palette::palettetypes pal)
{
  palette ret;

  int i, r, g, b;
  float f;

  switch (pal)
  {
    case palette::Linear_red_palettes:
      /*
       * Linear red palettes.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = 0;
        ret.colors[i].rgbGreen = 0;
        ret.colors[i].rgbRed = i;
      }
      break;
    case palette::GammaLog_red_palettes:
      /*
       * GammaLog red palettes.
       */
      for (i = 0; i < 256; i++)
      {
        f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
        ret.colors[i].rgbBlue = 0;
        ret.colors[i].rgbGreen = 0;
        ret.colors[i].rgbRed = f;
      }
      break;
    case palette::Inversion_red_palette:
      /*
       * Inversion red palette.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = 0;
        ret.colors[i].rgbGreen = 0;
        ret.colors[i].rgbRed = 255 - i;
      }
      break;
    case palette::Linear_palettes:
      /*
       * Linear palettes.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = i;
      }
      break;
    case palette::GammaLog_palettes:
      /*
       * GammaLog palettes.
       */
      for (i = 0; i < 256; i++)
      {
        f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
        ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = f;
      }
      break;
    case palette::Inversion_palette:
      /*
       * Inversion palette.
       */
      for (i = 0; i < 256; i++)
      {
        ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = 255 - i;
      }
      break;
    case palette::False_color_palette1:
      /*
       * False color palette #1.
       */
      for (i = 0; i < 256; i++)
      {
        r = (sin((i / 255.0 * 360.0 - 120.0 > 0 ? i / 255.0 * 360.0 - 120.0 : 0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        g = (sin((i / 255.0 * 360.0 + 60.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        b = (sin((i / 255.0 * 360.0 + 140.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        ret.colors[i].rgbBlue = b;
        ret.colors[i].rgbGreen = g;
        ret.colors[i].rgbRed = r;
      }
      break;
    case palette::False_color_palette2:
      /*
       * False color palette #2.
       */
      for (i = 0; i < 256; i++)
      {
        r = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        g = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        b = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        ret.colors[i].rgbBlue = b;
        ret.colors[i].rgbGreen = g;
        ret.colors[i].rgbRed = r;
      }
      break;
    case palette::False_color_palette3:
      /*
       * False color palette #3.
       */
      for (i = 0; i < 256; i++)
      {
        r = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        g = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        b = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
        ret.colors[i].rgbBlue = b;
        ret.colors[i].rgbGreen = g;
        ret.colors[i].rgbRed = r;
      }
      break;

    case palette::False_color_palette4:
      /*
       * False color palette #4. Matlab JET
       */

      enum
      {
        nsep = 64, nvals = 192, n = 256
      };

      std::vector<double> vals;
      vals.resize(nvals, 0);

      int idx = 0;
      for (int i = 0; i < nsep; ++i)
      {
        vals.at(idx++) = (i / (double)nsep);
      }

      for (int i = 0; i < nsep; ++i){
        vals.at(idx + i) = 1.;
      }

      idx += nsep;
      for (int i = nsep - 1; i >= 0; --i)
      {
        vals.at(idx++) = i / (double)nsep;
      }

      std::vector<int> r;
      r.resize(nvals);
      std::vector<int> g;
      g.resize(nvals);
      std::vector<int> b;
      b.resize(nvals);
      for (std::size_t i = 0; i < nvals; ++i)
      {
        g.at(i) = ceil(nsep / 2) - 1 + i;
        r.at(i) = g.at(i) + nsep;
        b.at(i) = g.at(i) - nsep;
      }

      int idxr = 0;
      int idxg = 0;

      for (int i = 0; i < nvals; ++i)
      {
        if (r.at(i) >= 0 && r.at(i) < n)
          ret.colors[r.at(i)].rgbRed = vals.at(idxr++) * 255.;

        if (g.at(i) >= 0 && g.at(i) < n)
          ret.colors[g.at(i)].rgbGreen = vals.at(idxg++) * 255.;
      }

      int idxb = 0;
      int cntblue = 0;
      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          cntblue++;
      }

      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          ret.colors[b.at(i)].rgbBlue = vals.at(nvals - 1 - cntblue + idxb++) * 255.;
      }
      break;
  }
  return ret;
}

palette GetPalette(const std::string &pal_choice)
{
  palette ret;

  int i, r, g, b;
  float f;

  if(pal_choice == "Linear_red_palettes")
  {
    /*
      * Linear red palettes.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = 0;
      ret.colors[i].rgbGreen = 0;
      ret.colors[i].rgbRed = i;
    }
  }
  else if(pal_choice == "GammaLog_red_palettes")
  {
    /*
      * GammaLog red palettes.
      */
    for (i = 0; i < 256; i++)
    {
      f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
      ret.colors[i].rgbBlue = 0;
      ret.colors[i].rgbGreen = 0;
      ret.colors[i].rgbRed = f;
    }
  }
  else if(pal_choice == "Inversion_red_palette")
  {
    /*
      * Inversion red palette.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = 0;
      ret.colors[i].rgbGreen = 0;
      ret.colors[i].rgbRed = 255 - i;
    }
  }
  else if(pal_choice == "Linear_palettes")
  {
    /*
      * Linear palettes.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = i;
    }
  }
  else if(pal_choice == "GammaLog_palettes")
  {
    /*
      * GammaLog palettes.
      */
    for (i = 0; i < 256; i++)
    {
      f = log10(pow((i / 255.0), 1.0) * 9.0 + 1.0) * 255.0;
      ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = f;
    }
  }
  else if(pal_choice == "Inversion_palette")
  {
    /*
      * Inversion palette.
      */
    for (i = 0; i < 256; i++)
    {
      ret.colors[i].rgbBlue = ret.colors[i].rgbGreen = ret.colors[i].rgbRed = 255 - i;
    }
  }
  else if(pal_choice == "False_color_palette1")
  {
    /*
      * False color palette #1.
      */
    for (i = 0; i < 256; i++)
    {
      r = (sin((i / 255.0 * 360.0 - 120.0 > 0 ? i / 255.0 * 360.0 - 120.0 : 0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      g = (sin((i / 255.0 * 360.0 + 60.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      b = (sin((i / 255.0 * 360.0 + 140.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      ret.colors[i].rgbBlue = b;
      ret.colors[i].rgbGreen = g;
      ret.colors[i].rgbRed = r;
    }
  }
  else if(pal_choice == "False_color_palette2")
  {
    /*
      * False color palette #2.
      */
    for (i = 0; i < 256; i++)
    {
      r = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      g = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      b = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      ret.colors[i].rgbBlue = b;
      ret.colors[i].rgbGreen = g;
      ret.colors[i].rgbRed = r;
    }
  }
  else if(pal_choice == "False_color_palette3")
  {
    /*
      * False color palette #3.
      */
    for (i = 0; i < 256; i++)
    {
      r = (sin((i / 255.0 * 360.0 + 240.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      g = (sin((i / 255.0 * 360.0 + 0.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      b = (sin((i / 255.0 * 360.0 + 120.0) * DEG2RAD) * 0.5 + 0.5) * 255.0;
      ret.colors[i].rgbBlue = b;
      ret.colors[i].rgbGreen = g;
      ret.colors[i].rgbRed = r;
    }
  }
  else if(pal_choice == "False_color_palette4")
  {
      /*
       * False color palette #4. Matlab JET
       */

      enum
      {
        nsep = 64, nvals = 192, n = 256
      };

      std::vector<double> vals;
      vals.resize(nvals, 0);

      int idx = 0;
      for (int i = 0; i < nsep; ++i)
      {
        vals.at(idx++) = (i / (double)nsep);
      }

      for (int i = 0; i < nsep; ++i){
        vals.at(idx + i) = 1.;
      }

      idx += nsep;
      for (int i = nsep - 1; i >= 0; --i)
      {
        vals.at(idx++) = i / (double)nsep;
      }

      std::vector<int> r;
      r.resize(nvals);
      std::vector<int> g;
      g.resize(nvals);
      std::vector<int> b;
      b.resize(nvals);
      for (std::size_t i = 0; i < nvals; ++i)
      {
        g.at(i) = ceil(nsep / 2) - 1 + i;
        r.at(i) = g.at(i) + nsep;
        b.at(i) = g.at(i) - nsep;
      }

      int idxr = 0;
      int idxg = 0;

      for (int i = 0; i < nvals; ++i)
      {
        if (r.at(i) >= 0 && r.at(i) < n)
          ret.colors[r.at(i)].rgbRed = vals.at(idxr++) * 255.;

        if (g.at(i) >= 0 && g.at(i) < n)
          ret.colors[g.at(i)].rgbGreen = vals.at(idxg++) * 255.;
      }

      int idxb = 0;
      int cntblue = 0;
      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          cntblue++;
      }

      for (int i = 0; i < nvals; ++i)
      {
        if (b.at(i) >= 0 && b.at(i) < n)
          ret.colors[b.at(i)].rgbBlue = vals.at(nvals - 1 - cntblue + idxb++) * 255.;
      }
  }
  return ret;
}
#undef DEG2RAD

void convertFalseColor(const cv::Mat& srcmat, cv::Mat& dstmat, const palette &pal, bool drawlegend, double mintemp, double maxtemp)
{
  dstmat.create(srcmat.rows, srcmat.cols, CV_8UC3);

  cv::Size sz = srcmat.size();
  const unsigned char* src = srcmat.data;
  unsigned char* dst = dstmat.data;

  if (srcmat.isContinuous() && dstmat.isContinuous())
  {
    sz.width *= sz.height;
    sz.height = 1;
  }

  for (int i = 0; i < sz.width; ++i)
  {
    for (int j = 0; j < sz.height; ++j)
    {
      int idx = j * sz.width + i;
      uint8_t val = src[idx];
      dst[idx * dstmat.channels() + 0] = pal.colors[val].rgbBlue;
      dst[idx * dstmat.channels() + 1] = pal.colors[val].rgbGreen;
      dst[idx * dstmat.channels() + 2] = pal.colors[val].rgbRed;
    }
  }

  //draw a legend if true Temperatures
  if(drawlegend){

    //get min max to scale the legend
    double max_val;
    double min_val;
    cv::minMaxIdx(srcmat, &min_val, &max_val);

    enum{
      legenddiscretization = 5,
      legendnumbers = 5,
      legendwidth = 5,
      x_0 = 20,
      y_0 = 10,
    };
    double stepsize;

    //    std::cout<<"mintemp "<<mintemp<<" maxtemp "<<maxtemp<<std::endl;

    //draw legend color bar
    for(int i = y_0 ; i < dstmat.rows - y_0 ; ++i){
      int py = dstmat.rows - i;
      int val = (i - y_0) / (double)(dstmat.rows - y_0 * 2) * 255.;
      cv::rectangle(dstmat, cv::Point(x_0, py), cv::Point(x_0 + legendwidth, py + 1),
                    CV_RGB(pal.colors[val].rgbRed, pal.colors[val].rgbGreen, pal.colors[val].rgbBlue ), -1);
    }

    //draw temp tick labels
    stepsize = (dstmat.rows - y_0 * 2) / (double)legendnumbers;
    for(int i = 0 ; i <= legendnumbers ; ++i){
      int py = y_0 + (legendnumbers - i) * stepsize + 5; //bottom up
      double tempval = (mintemp - 273.15) + i * (maxtemp - mintemp) / (double)legendnumbers;
      std::stringstream ss;
      ss<<std::setprecision(2)<<tempval<<" C";
      cv::putText(dstmat, ss.str(), cv::Point(x_0 + 20, py), CV_FONT_HERSHEY_SIMPLEX, 0.4, CV_RGB(255,255,255), 1);
    }

    //draw ticks into legends
    stepsize = (dstmat.rows - y_0 * 2) / (double)legenddiscretization;
    for(int i = 0 ; i <= legenddiscretization ; ++i){
      int py = y_0 + (legenddiscretization - i) * stepsize; //bottom up
      cv::line(dstmat, cv::Point(x_0 - 2, py), cv::Point(x_0 + legendwidth + 2, py), CV_RGB(255,255,255), 1);
    }
  }

}

converter_16_8::converter_16_8()
{
  min_ = std::numeric_limits<uint16_t>::max();
  max_ = 0;
  firstframe_ = true;
}

converter_16_8::~converter_16_8()
{
  delete inst_;
  inst_ = NULL;
}

double converter_16_8::getMin(){
  return min_;
}
double converter_16_8::getMax(){
  return max_;
}
// void converter_16_8::toneMapping(const cv::Mat& img16, cv::Mat& img8){
//   if(!retina_){
//     retina_.reset(new cv::Retina(img16.size(), false));
//     retina_->setup(ros::package::getPath("brisk") + "/include/flir/retina_params");
//   }
//   retina_->run(img16);
//   retina_->getParvo(img8);
// }

//adjustment -3 for slightly wrong fit
double powerToK4(double power){
  double slope = 2.58357167114001779457e-07;
  double y_0 = 2.26799217314804718626e+03;
return sqrt(sqrt(((double)power - y_0) / slope)) - 3;
}

void converter_16_8::convert_to8bit(const cv::Mat& img16, cv::Mat& img8, bool doTempConversion)
{
  if(img8.empty()){ //make an image if the user has provided nothing
    img8.create(cvSize(img16.cols, img16.rows), CV_8UC1);
  }

  double min = std::numeric_limits<uint16_t>::max();
  double max = 0;

  //make a histogram of intensities
  typedef std::map<double, int> hist_t;
  hist_t hist;

  double bucketwidth = 2.; //bucketwidth in degrees K

  for (int i = 0; i < img16.cols; ++i)
  {
    for (int j = 0; j < img16.rows; ++j)
    {
      double power = img16.at<uint16_t>(j, i);
      double temp;
      if(doTempConversion){
        temp = powerToK4(power);
      }else{
        temp = power;
      }
      temp = round(temp / bucketwidth) * bucketwidth;
      hist[temp]++;
    }
  }

  //find the main section of the histogram
  for (hist_t::const_iterator it = hist.begin(); it != hist.end(); ++it)
  {
    if (it->second > histminmembersperbucket)
    {
      if (it->first > max)
      {
        max = it->first;
      }
      if (it->first < min)
      {
        min = it->first;
      }
    }
  }

  if (firstframe_)
  {
    min_ = min;
    max_ = max;
  }

  //  std::cout<<"min: "<<min-273.15<<" max: "<<max-273.15<<" sm: min: "<<min_-273.15<<" max: "<<max_-273.15<<std::endl;

  //exp smoothing
  double expsm = 0.95;
  min_ = expsm * min_ + (1. - expsm) * min;
  max_ = expsm * max_ + (1. - expsm) * max;

  for (int i = 0; i < img16.cols; ++i)
  {
    for (int j = 0; j < img16.rows; ++j)
    {
      double temp;
      if(doTempConversion){
        temp = powerToK4(img16.at<uint16_t>(j, i));
      }else{
        temp = (double)(img16.at<uint16_t>(j, i));
      }

      int val = (((temp - min_) / (max_ - min_)) * 255);

      val = val > std::numeric_limits<uint8_t>::max() ? std::numeric_limits<uint8_t>::max() : val < 0 ? 0 : val; //saturate
      img8.at<uint8_t>(j, i) = (uint8_t)val;
    }
  }

  firstframe_ = false;
}

converter_16_8* converter_16_8::inst_ = NULL;


#include "biggles/observation.hpp"
#include "biggles/samplers.hpp"
#include "biggles/observation_collection.hpp"
#include "biggles/track.hpp"
#include "biggles/track_collection.hpp"
#include "biggles/partition.hpp"
#include "biggles/detail/random.hpp"
#include "biggles/sampling/simple.hpp"
#include <boost/foreach.hpp>
#include <boost/math/distributions.hpp>
#include <boost/timer.hpp>
#include <iomanip>
#include <iostream>
#include <iterator>
#include <fstream>
#include <algorithm>
#include <numeric>
#include <string>

#include "biggles/tools/sundries.hpp"

// include this last to stop pre-processor macros breaking things
extern "C" {
#include <ccan/tap/tap.h>
}

using namespace biggles;

void print_obs(observation obs) {
    std::cout << "x=" << x(obs) << ", y= " << y(obs) << ", t= " << t(obs);
}

typedef std::deque<float> data_container;
typedef std::pair<float, float> data_pair;

float sample_beta_alt(float alpha, float beta) {
    boost::math::beta_distribution<float> dist(alpha, beta);
    return quantile(dist, sampling::uniform_real());
}

int test_normal(int total) {
    diag("Normal (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_normal();
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    diag(" mean %.2f, stdev %.2f", ms.first, ms.second);
    return 0;
}

int test_uniform() {
    diag("Uniform 1 ");
    float minimum = 1.f;
    float maximum = 0.f;
    float current = 0.f;
    size_t ctr = 0;
    while (minimum > 0.f and maximum < 1.f) {
        current = sampling::uniform_real();
        if (current < minimum) {
            minimum = current;
            diag("  (%g, %g)", minimum, 1.f - maximum);
        }
        if (current > maximum) {
            maximum = current;
            diag("  (%g, %g)", minimum, 1.f - maximum);
        }
        ++ctr;
    }
    diag(" max == 1 ? %d",  maximum == 1);
    diag(" min == 0 ? %d",  minimum == 0);
    diag(" nloops = %zu", ctr);
    return 0;
}

int test_uniform_inv() {
    diag("Uniform inverse ");
    float minimum = 1.f;
    float maximum = 0.f;
    float current = 0.f;
    int ctr = 0;
    while (minimum > 0.f and maximum < 1.f) {
        current = 1.f - sampling::uniform_real();
        if (current < minimum) {
            minimum = current;
        }
        if (current > maximum) {
            maximum = current;
        }
        ++ctr;
    }
    diag(" max == 1 ? %d",  maximum == 1);
    diag(" min == 0 ? %d",  minimum == 0);
    diag(" nloops = %d", ctr);
    return 0;
}

int test_factorial() {
    model::log_factorial &fact = model::log_factorial::get();
    diag("  6! = %f", fact(6));
    diag(" 12! = %f", fact(12));
    diag("  8! = %f", fact(8));
    diag("  3! = %f", fact(3));
    diag("  0! = %f", fact(0));
    diag("  1! = %f", fact(1));
    diag("113! = %f", fact(113));
    return 0;
}

int test_trunc_beta(int total) {
    diag("Truncated Beta (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_truncated_beta(1.f+0.0001f,1.f + 40.0001f, 0.8, 1.0);
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    diag(" mean %.6f, stdev %.6f, min %.6f, max %.6f", ms.first, ms.second,
        *std::min_element(samples.begin(), samples.end()),
        *std::max_element(samples.begin(), samples.end()));
    return 0;
}

int test_beta(int total) {
    diag("Beta (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_beta(1.f+10.f,1.f + 10.f);
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    diag(" mean %.6f, stdev %.6f", ms.first, ms.second);
    return 0;
}

int test_beta2(int total) {
    diag("Beta2 (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_beta(1.f+10.f,1.f + 10.f);
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    diag(" mean %.6f, stdev %.6f", ms.first, ms.second);
    return 0;
}

int test_beta3(int total) {
    diag("Beta3 (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_beta(1.0001f+2785.f,1.0001f);
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    diag(" mean %.6f, stdev %.6f", ms.first, ms.second);
    return 0;
}

int test_gamma(int total) {
    diag("Gamma (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_gamma(1.f+71.f,1.f);
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    ok1(size_t(std::count_if(samples.begin(), samples.end(), std::isfinite<float>)) == samples.size());
    diag(" mean %.2f, stdev %.2f", ms.first, ms.second);
    return 0;
}

int test_gamma0(int total) {
    diag("Gamma 0 (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_gamma(1.f,1.f);
        if (not std::isfinite(samples[i])) {
            diag("value is not finite");
        }
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    ok1(size_t(std::count_if(samples.begin(), samples.end(), std::isfinite<float>)) == samples.size());
    diag(" mean %.5f, stdev %.5f", ms.first, ms.second);
    return 0;
}

int test_gamma3(int total) {
    diag("Gamma 3 (%d)",total);
    data_container samples(total);
    for (int i = 0; i < total; ++i) {
        samples[i] = sampling::sample_gamma(1.f,1.f);
        if (not std::isfinite(samples[i])) {
            diag("value is not finite");
        }
    }
    data_pair ms = mean_stdev(samples.begin(), samples.end());
    ok1(size_t(std::count_if(samples.begin(), samples.end(), std::isfinite<float>)) == samples.size());
    diag(" mean %.5f, stdev %.5f", ms.first, ms.second);
    return 0;
}

int test_gamma1() {
    diag("Gamma 1");
    float minimum = 1.f;
    float current = 0.f;
    size_t ctr = 0;
    while (minimum > 0.f) {
        current = sampling::sample_gamma(1.f,1.f);
        if (current < minimum) {
            minimum = current;
            //diag("  min = %g", minimum);
        }
        ++ctr;
    }
    diag(" nloops = %zu", ctr);
    return 0;
}

int test_gamma2() {
    diag("Gamma 2");
    float minimum = 1.f;
    float current = 0.f;
    int ctr = 0;
    while (minimum > 0.f) {
        if (ctr == std::numeric_limits<int>::max())
            break;
        current = sampling::sample_gamma0(1.f,1.f);
        if (current < minimum) {
            minimum = current;
            diag("min = %g", minimum);
        }
        ++ctr;
    }
    diag(" nloops = %d", ctr);
    return 0;
}

int test_clutter_rate_sampling(int total) {
    observation_collection obs_coll;
    for (time_stamp ts = 0; ts < 50; ++ts)
        obs_coll.insert(new_obs(0.f, 0.f, ts));
    boost::shared_ptr<track_collection> tracks(new track_collection());
    clutter_ptr clutter(new clutter_t(obs_coll.begin(), obs_coll.end()));
    const partition part(tracks, clutter);
    diag("partition duration = %zu", part.duration());
    diag("clutter size = %zu", part.clutter().size());
    model::parameters para;
    for (int i = 0; i < total; ++i) {
        try {
            sample_clutter_rate_given_partition(part, para);
        }
        catch (const std::exception &e) {
            diag("loop %d", i);
        }
    }
    return 0;
}

int main(int argc, char** argv)
{
    unsigned int seed=std::time(0);
    // seed the PRNG to ensure consistent results across runs.
    // biggles::detail::seed_prng(0xfacedead);
    //seed = 0x55fa8e3d;
    biggles::detail::seed_prng(seed);

    bool verbose = (argc > 1) && (0 == strcmp(argv[1], "-v"));

    plan_tests(13+3-1);

#ifdef NDEBUG
    diag("NDEBUG is defined");
#else
    diag("NDEBUG is **not** defined");
#endif

    observation_collection cl_obs;

    cl_obs.insert(new_obs(-3, 0, 0));
    cl_obs.insert(new_obs(-2, 0, 1));
    //cl_obs.insert(new_obs(-1, 0, 2));
    cl_obs.insert(new_obs( 0, 0, 3));
    cl_obs.insert(new_obs( 1, 0, 4));
    cl_obs.insert(new_obs( 2, 0, 5));
    cl_obs.insert(new_obs( 3, 0, 6));

    observation_collection collection2;
    collection2.insert(new_obs( 0, 0, 1));
    collection2.insert(new_obs( 0, 0.1, 3));
    collection2.insert(new_obs( 0, 0.2, 3));
    collection2.insert(new_obs( 0, 0.3, 3));
    collection2.insert(new_obs( .1, 0, 3));
    collection2.insert(new_obs( .2, 0, 3));

    //const float sqrt2 = sqrtf(2.0f);

    const int total = 10000;
    const float lambda = 2.5f;

    data_container poisson_samples(total);

    for (int i = 0; i < total; ++i) {
        poisson_samples[i] = sampling::poisson(lambda);
    }

    data_pair pois_ms = mean_stdev(poisson_samples.begin(), poisson_samples.end());
    data_pair pois_minmax = std::make_pair(*std::min_element(poisson_samples.begin(), poisson_samples.end()),
        *std::max_element(poisson_samples.begin(), poisson_samples.end()));
    if (verbose) {
        std::cout
            << "Poisson distribution: " << std::endl
            << "mean = " << pois_ms.first
            << ", "
            << "stdev = " << pois_ms.second
            << ", "
            << "stdev*stdev = " << (pois_ms.second * pois_ms.second)
            << ", "
            << "min = " << pois_minmax.first
            << ", "
            << "max = " << pois_minmax.second
            << std::endl;
        ;
    }

    ok(*std::min_element(poisson_samples.begin(), poisson_samples.end()) == 0, "min Poisson == 0");
    ok(fabsf(lambda - pois_ms.first) < 0.1, "Poisson: sample mean near lambda");

    data_container exponential_samples(total);

    for (int i = 0; i < total; ++i) {
        exponential_samples[i] = sampling::exponential(lambda);
    }

    data_pair exp_ms = mean_stdev(exponential_samples.begin(), exponential_samples.end());
    data_pair exp_minmax = std::make_pair(*std::min_element(exponential_samples.begin(), exponential_samples.end()),
        *std::max_element(exponential_samples.begin(), exponential_samples.end()));
    if (verbose) {
        std::cout
            << "Exponential distribution: " << std::endl
            << "mean = " << exp_ms.first
            << ", "
            << "stdev = " << exp_ms.second
            << ", "
            << "stdev*stdev = " << (exp_ms.second * exp_ms.second)
            << ", "
            << "min = " << exp_minmax.first
            << ", "
            << "max = " << exp_minmax.second
            << std::endl;
        ;
    }

    ok(*std::min_element(exponential_samples.begin(), exponential_samples.end()) >= 0.0f, "min Exponential >= 0");
    ok(*std::min_element(exponential_samples.begin(), exponential_samples.end()) < 0.01f, "min Exponential near 0");
    ok(fabsf(1.0f/lambda - exp_ms.first) < 0.1, "Exponential: sample mean near lambda");

    float half_pois = ceilf(pois_minmax.second/2.0);
    bool all_ok = true;
    for (int num = 0 ; num<= int(half_pois) ; ++num) {
        bool done = false;
        for (int i = 0; i < total; ++i) {
            if (num == *sampling::from_range(poisson_samples.begin(), poisson_samples.end())) {
                done = true;
                if (verbose) {
                    std::cout
                        << "from_range "
                        << num << " -- " << i
                        << std::endl
                    ;
                }
                break;
            }
        }
        all_ok = all_ok and done;
    }
    ok(all_ok, "from_range sampling");

    const int repeats = 100;
    int count_ok = 0;
    const float from_range_prob = -logf(float(total));
    for (int i=0; i < repeats; ++i) {
        float prob = 0.f;
        sampling::from_range(poisson_samples.begin(), poisson_samples.end(), prob);
        count_ok += int(prob == from_range_prob);
    }
    ok(count_ok == repeats, "from_range_prob");

    std::vector<int> unif_ints(repeats);

    const int i_upper = 5;
    for (int i=0; i < repeats; ++i) {
        unif_ints[i] = sampling::uniform_int(0, i_upper);
    }
    bool unif_sampling_ok = true;
    for (int i=0; i < i_upper; ++i) {
        unif_sampling_ok = unif_sampling_ok and (std::count(unif_ints.begin(), unif_ints.end(), i) > 0);
    }
    unif_sampling_ok = unif_sampling_ok and (std::count(unif_ints.begin(), unif_ints.end(), i_upper) == 0);
    ok(unif_sampling_ok, "uniform integer sampling");

    float count_0 = 0;
    float count_1 = 0;
    //const int total = 100000;
    for (int i = 0; i < total; ++i) {
        int isam = sampling::uniform_int(0, 2);
        count_1 += isam == 1 ? 1.f : 0.f;
        count_0 += isam == 0 ? 1.f : 0.f;
    }
    count_0 /= float(total);
    count_1 /= float(total);
    ok1(count_0 > 0.48 and count_1 > 0.48 and count_0 + count_1 == 1.f);

    const float f_upper = 5.0f;
    unif_sampling_ok = true;
    data_container unif_floats(repeats);
    for (int i=0; i < repeats; ++i) {
        unif_floats[i] = sampling::uniform_real(0, f_upper);
    }
    unif_sampling_ok = unif_sampling_ok and *std::min_element(unif_floats.begin(), unif_floats.end()) < 1.0f;
    unif_sampling_ok = unif_sampling_ok and *std::min_element(unif_floats.begin(), unif_floats.end()) >= 0.0f;
    unif_sampling_ok = unif_sampling_ok and *std::max_element(unif_floats.begin(), unif_floats.end()) < f_upper;
    ok(unif_sampling_ok, "uniform float sampling");

    observation centre(new_obs(0, 0, 3));
    observation sample(new_obs(0, 0, 0));
    //float prob;

    const float eps = std::numeric_limits<float>::epsilon();

    const float speed_of_light(1.0f + eps);

    std::set<time_stamp> time_stamp_with_obs;
    observation_collection candidates;
    observations_within_light_cone(cl_obs, centre, speed_of_light,
                               1, 6,
                               time_stamp_with_obs,
                               std::inserter(candidates, candidates.begin()));
    if (verbose)
        std::cout << "candidates size = " << candidates.size() << std::endl;
    ok(candidates.size() == 4, "observations_within_light_cone");

    //int num_sampling_success = 0;
    /*
    observation start(new_obs(0, 0, 0));
    for (int i = 0 ; i < total; ++i) {
        sample = new_obs(-1, -1, -1);
        prob = 0.0f;
        bool success = sampling::observation_from_light_cone(collection2, start, speed_of_light, 1, 4, sample, prob);
        if (not (t(sample) == 1 and prob == logf(0.5f)) and not (t(sample) == 3 and prob == logf(0.1f))) {
            success = false;
        }
        num_sampling_success += success ? 1 : 0;
    }
    ok(num_sampling_success == total, "observation_from_light_cone successful");
    */

    /*
    num_sampling_success = 0;
    for (int i = 0 ; i < total; ++i) {
        sample = new_obs(-1, -1, -1);
        prob = 0.0f;
        bool success = sampling::observation_from_collection(collection2, sample, prob);
        if (not (t(sample) == 1 and prob == logf(0.5f)) and not (t(sample) == 3 and prob == logf(0.1f))) {
            //std::cout << t(sample) << "; p=" << expf(prob) << std::endl;
            success = false;
        }
        num_sampling_success += success ? 1 : 0;
    }
    ok(num_sampling_success == total, "observation_from_collection");
    */
    ok(true, "observation_from_collection not longer in use");

    test_normal(total);

    /*
    boost::timer timer;
    int nloops = 10000000;
    test_beta(nloops);
    diag("time 1st beta test: %f", timer.elapsed());
    timer.restart();
    test_beta2(nloops);
    diag("time 2nd beta test: %f", timer.elapsed());
    */
    test_beta3(1000);
    test_gamma(1000);
    test_gamma0(1000);
    test_gamma3(1000);
    test_gamma1();
    //test_trunc_beta(10000);
    test_factorial();
    //test_uniform();
    //test_uniform_inv();
    //test_gamma2();
    //test_clutter_rate_sampling(1000000);
    diag("random seed = 0x%x", seed);
    return exit_status();
}


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file lp_row_generation.hpp
 * @brief
 * @author Piotr Godlewski, Robert Rosołek
 * @version 1.0
 * @date 2013-06-04
 */
#ifndef PAAL_LP_ROW_GENERATION_HPP
#define PAAL_LP_ROW_GENERATION_HPP

#include "paal/lp/lp_base.hpp"
#include "paal/lp/problem_type.hpp"
#include "paal/utils/rotate.hpp"

#include <boost/range/counting_range.hpp>

namespace paal {
namespace lp {

/**
 * Finds an extreme point solution to the LP using row generation:
 * solves the initial LP and then ask the separation oracle if the found
 * solution is a feasible solution to the complete problem. If not,
 * adds a new row (generated by the oracle) to the LP and re-optimizes it.
 * This procedure is iterated until a feasible solution to the full LP
 * is found.
 */
template <class TryAddViolated, class SolveLp>
    problem_type row_generation(TryAddViolated try_add_violated, SolveLp solve_lp)
    {
        problem_type res;
        do res = solve_lp(); while (res == OPTIMAL && try_add_violated());
        return res;
    }

/**
 * @brief functor for adding maximum violated constraint
 *
 * @tparam GetCandidates
 * @tparam HowViolated
 * @tparam AddViolated
 * @tparam CompareHow
 */
template<
    class GetCandidates,
    class HowViolated,
    class AddViolated,
    class CompareHow
>
class add_max_violated {
   GetCandidates m_get_candidates;
   HowViolated m_how_violated;
   AddViolated m_add_violated;
   CompareHow m_cmp;

   public:
      ///contructor
      add_max_violated(GetCandidates get_candidates,
            HowViolated how_violated, AddViolated add_violated, CompareHow cmp)
         : m_get_candidates(get_candidates), m_how_violated(how_violated),
            m_add_violated(add_violated), m_cmp(cmp) {}

      ///operator()
      bool operator()() {
         auto&& cands = m_get_candidates();
         using how_violated_t = puretype(m_how_violated(*std::begin(cands)));
         using cand_it_t = puretype(std::begin(cands));
         boost::optional<std::pair<how_violated_t, cand_it_t>> most;
         for (auto cand : boost::counting_range(cands)) {
            auto const how = m_how_violated(*cand);
            if (!how) continue;
            if (!most || m_cmp(most->first, how))
               most = std::make_pair(std::move(how), cand);
         }
         if (!most) return false;
         m_add_violated(*most->second);
         return true;
      }
};

///functor computing add_max_violated
struct max_violated_separation_oracle {
   template <
      class GetCandidates,
      class HowViolated,
      class AddViolated,
      class CompareHow = utils::less
   >
   ///operator()
   auto operator()(
      GetCandidates get_candidates,
      HowViolated is_violated,
      AddViolated add_violated,
      CompareHow compare_how = CompareHow{}
   ) const {
      return add_max_violated<GetCandidates, HowViolated, AddViolated,
         CompareHow>(get_candidates, is_violated, add_violated, compare_how);
   }
};

///functor
template <class GetCandidates,
          class HowViolated,
          class AddViolated,
          class ReorderCandidates>
class add_first_violated {
   GetCandidates m_get_candidates;
   HowViolated m_how_violated;
   AddViolated m_add_violated;
   ReorderCandidates m_reorder_candidates;

   public:
      ///constructor
      add_first_violated(
         GetCandidates get_candidates,
         HowViolated how_violated,
         AddViolated add_violated,
         ReorderCandidates reorder_candidates
      ) : m_get_candidates(get_candidates),
         m_how_violated(how_violated),
         m_add_violated(add_violated),
         m_reorder_candidates(std::move(reorder_candidates)) {}

      ///operator()
      bool operator()() {
         auto&& cands = m_get_candidates();
         auto reordered =
            m_reorder_candidates(std::forward<decltype(cands)>(cands));
         for (auto c : boost::counting_range(reordered)) {
            if (m_how_violated(*c)) {
                m_add_violated(*c);
                return true;
            }
         }
         return false;
      }
};

///functor computing add_first_violated
struct first_violated_separation_oracle {
   template <
      class GetCandidates,
      class HowViolated,
      class AddViolated,
      class ReorderCandidates = utils::identity_functor
   >
   ///operator()
   auto operator() (
      GetCandidates get_candidates,
      HowViolated how_violated,
      AddViolated add_violated,
      ReorderCandidates reorder_candidates = ReorderCandidates{}
   ) const {
      return add_first_violated<GetCandidates, HowViolated, AddViolated,
         ReorderCandidates>(get_candidates, how_violated, add_violated,
            reorder_candidates);
   }
};

namespace detail {
template <class URNG>
class random_rotate {
   URNG m_g;
   public:
      random_rotate(URNG&& g)
         : m_g(std::forward<URNG>(g)) {}
      template <class ForwardRange>
      auto operator()(const ForwardRange& rng)
      {
         auto const len = boost::distance(rng);
         std::uniform_int_distribution<decltype(len)> d(0, len);
         return utils::rotate(rng, d(m_g));
      }
};

template <class URNG = std::default_random_engine>
auto make_random_rotate(URNG&& g = URNG{})
{
   return random_rotate<URNG>(std::forward<URNG>(g));
}
} //! detail

///functor returning add_first_violated
///Separation oracle for the row generation,
///using the random violated strategy.
struct random_violated_separation_oracle {
   template <
      class GetCandidates,
      class HowViolated,
      class AddViolated,
      class URNG = std::default_random_engine
   >
   ///operator()
   auto operator() (
      GetCandidates get_candidates,
      HowViolated how_violated,
      AddViolated add_violated,
      URNG&& g = URNG{}
   ) const {
      return first_violated_separation_oracle{}(get_candidates,
            how_violated, add_violated, detail::make_random_rotate(std::forward<URNG>(g)));
   }
};

} // lp
} // paal

#endif // PAAL_LP_ROW_GENERATION_HPP


#pragma once

#include <Eigen/Core>
#include <vector>

namespace GooBalls{

template <typename Derived>
void minSize(Eigen::DenseBase<Derived>& a, int N){
    if(a.rows() >= N){
        return;
    }
    a.derived().resize(N, Eigen::NoChange);
}

template <typename Derived>
void minSize(Eigen::DenseBase<Derived>& a, int N, int M){
    if(a.rows() >= N && a.cols() >= M){
        return;
    }
    a.derived().resize(N, M);
}

template <typename Derived1, typename Derived2>
void pickRows(
        const Eigen::DenseBase<Derived1>& data,
        const std::vector<int> indexes,
        Eigen::DenseBase<Derived2> & out
        ) {
    minSize(out, indexes.size(), data.cols());
    for(size_t i = 0; i < indexes.size(); ++i){
        assert(indexes[i] < data.rows());
        out.row(i) = data.row(indexes[i]);
    }
}

}


#define BOOST_TEST_MODULE TestVector2
#include <boost/test/included/unit_test.hpp>

#include "../src/Vector2.hpp"
#include "../src/Vector3.hpp"
#include "../src/Vector4.hpp"
#include "../src/Quaternion.hpp"

#include "internal/PrintTVector2.hpp"

using namespace IOMath;

BOOST_AUTO_TEST_CASE(Vector2_DefaultConstructor)
{
    Vector2 vector = Vector2();

    BOOST_CHECK_EQUAL(vector, Vector2(0.0f, 0.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_Constructor)
{
    Vector2 vector = Vector2(1.0f, 2.7f);

    BOOST_CHECK_EQUAL(vector.x, 1.0f);
    BOOST_CHECK_EQUAL(vector.y, 2.7f);
}

BOOST_AUTO_TEST_CASE(Vector2_FromScalar)
{
    Vector2 vector = Vector2(2.2f);

    BOOST_CHECK_EQUAL(vector, Vector2(2.2f, 2.2f));
}

BOOST_AUTO_TEST_CASE(Vector2_FromVector3)
{
    Vector2 vector = Vector2::FromVector3(Vector3(1.0f, 2.0f, 3.0f));

    BOOST_CHECK_EQUAL(vector, Vector2(1.0f, 2.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_FromVector4)
{
    Vector2 vector = Vector2::FromVector4(Vector4(1.0f, 2.0f, 3.0f, 4.0f));

    BOOST_CHECK_EQUAL(vector, Vector2(1.0f, 2.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_FromQuaternion)
{
    Vector2 vector = Vector2::FromQuaternion(Quaternion::Identity());

    BOOST_CHECK_EQUAL(vector, Vector2(0.0f, 0.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_UnsafeBracketsOperator)
{
    Vector2 vector = Vector2(5.4f, 2.6f);

    BOOST_CHECK_EQUAL(vector, Vector2(vector[0], vector[1]));
}

BOOST_AUTO_TEST_CASE(Vector2_ChangingValueUsingUnsafeBracketsOperator)
{
    Vector2 vector = Vector2(5.4f, -1.1f);
    vector[0] = 1.2f;
    vector[1] /= -1.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(1.2f, 1.1f));
}

BOOST_AUTO_TEST_CASE(Vector2_ValueByIndex)
{
    Vector2 vector = Vector2(5.4f, 2.6f);

    BOOST_CHECK_EQUAL(vector, Vector2(vector.ValueByIndex(0), vector.ValueByIndex(1)));
}

BOOST_AUTO_TEST_CASE(Vector2_ValueByIndexOutOfBounds)
{
    Vector2 vector = Vector2(5.4f, 2.6f);

    BOOST_CHECK_EQUAL(vector, Vector2(vector.ValueByIndex(0), vector.ValueByIndex(256)));
}

BOOST_AUTO_TEST_CASE(Vector2_ChangingValueByIndex)
{
    Vector2 vector = Vector2(5.6f, 3.8f);
    vector.ValueByIndex(0) *= 0.5f;
    vector.ValueByIndex(1) *= 2.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(2.8f, 7.6f));
}

BOOST_AUTO_TEST_CASE(Vector2_ChangingValueByIndexOutOfBounds)
{
    Vector2 vector = Vector2(5.6f, 3.8f);
    vector.ValueByIndex(0) *= 0.5f;
    vector.ValueByIndex(256) *= 2.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(2.8f, 7.6f));
}

BOOST_AUTO_TEST_CASE(Vector2_AddScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    Vector2 result = vector + 3.0f;

    BOOST_CHECK_EQUAL(result, Vector2(4.0f, 1.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_AddVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    Vector2 result = vector + Vector2(3.0f, -4.5f);

    BOOST_CHECK_EQUAL(result, Vector2(8.0f, 1.5f));
}

BOOST_AUTO_TEST_CASE(Vector2_AddAssignScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    vector += 3.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(4.0f, 1.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_AddAssignVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    vector += Vector2(3.0f, -4.5f);

    BOOST_CHECK_EQUAL(vector, Vector2(8.0f, 1.5f));
}

BOOST_AUTO_TEST_CASE(Vector2_SubtractScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    Vector2 result = vector - 3.0f;

    BOOST_CHECK_EQUAL(result, Vector2(-2.0f, -5.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_SubtractVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    Vector2 result = vector - Vector2(1.0f, -4.5f);

    BOOST_CHECK_EQUAL(result, Vector2(4.0f, 10.5f));
}

BOOST_AUTO_TEST_CASE(Vector2_SubtractAssignScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    vector -= 3.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(-2.0f, -5.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_SubtractAssignVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    vector -= Vector2(1.0f, -4.5f);

    BOOST_CHECK_EQUAL(vector, Vector2(4.0f, 10.5f));
}

BOOST_AUTO_TEST_CASE(Vector2_MultiplyByScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    Vector2 result = vector * 3.0f;

    BOOST_CHECK_EQUAL(result, Vector2(3.0f, -6.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_MultiplyByVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    Vector2 result = vector * Vector2(-1.0f, 4.5f);

    BOOST_CHECK_EQUAL(result, Vector2(-5.0f, 27.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_MultiplyAssignByScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    vector *= 3.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(3.0f, -6.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_MultiplyAssignByVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    vector *= Vector2(-1.0f, 4.5f);

    BOOST_CHECK_EQUAL(vector, Vector2(-5.0f, 27.0f));
}

BOOST_AUTO_TEST_CASE(Vector2_DivideByScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    Vector2 result = vector / 3.0f;

    BOOST_CHECK_EQUAL(result, Vector2(0.333333343f, -0.666666687f));
}

BOOST_AUTO_TEST_CASE(Vector2_DivideByVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    Vector2 result = vector / Vector2(-1.0f, 4.5f);

    BOOST_CHECK_EQUAL(result, Vector2(-5.0f, 1.33333337f));
}

BOOST_AUTO_TEST_CASE(Vector2_DivideAssignByScalar)
{
    Vector2 vector = Vector2(1.0f, -2.0f);
    vector /= 3.0f;

    BOOST_CHECK_EQUAL(vector, Vector2(0.333333343f, -0.666666687f));
}

BOOST_AUTO_TEST_CASE(Vector2_DivideAssignByVector)
{
    Vector2 vector = Vector2(5.0f, 6.0f);
    vector /= Vector2(-1.0f, 4.5f);

    BOOST_CHECK_EQUAL(vector, Vector2(-5.0f, 1.33333337f));
}

BOOST_AUTO_TEST_CASE(Vector2_Negative)
{
    Vector2 vector = -Vector2(-1.0f, 5.3f);

    BOOST_CHECK_EQUAL(vector, Vector2(1.0f, -5.3f));
}

BOOST_AUTO_TEST_CASE(Vector2_PrefixIncrement)
{
    IVector2 vector = IVector2(5, 1);

    BOOST_CHECK_EQUAL(++vector, IVector2(6, 2));
}

BOOST_AUTO_TEST_CASE(Vector2_PrefixDecrement)
{
    IVector2 vector = IVector2(5, 1);

    BOOST_CHECK_EQUAL(--vector, IVector2(4, 0));
}

BOOST_AUTO_TEST_CASE(Vector2_PostfixIncrement)
{
    IVector2 vector = IVector2(5, 1);

    BOOST_CHECK_EQUAL(vector++, IVector2(5, 1));
    BOOST_CHECK_EQUAL(vector, IVector2(6, 2));
}

BOOST_AUTO_TEST_CASE(Vector2_PostfixDecrement)
{
    IVector2 vector = IVector2(5, 1);

    BOOST_CHECK_EQUAL(vector--, IVector2(5, 1));
    BOOST_CHECK_EQUAL(vector, IVector2(4, 0));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_AndScalar)
{
    UVector2 vector = UVector2(2u, 4u);
    UVector2 result = vector & 2u;

    BOOST_CHECK_EQUAL(result, UVector2(2u, 0u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_AndVector)
{
    UVector2 vector = UVector2(4u, 1u);
    UVector2 result = vector & UVector2(1u, 1u);

    BOOST_CHECK_EQUAL(result, UVector2(0u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_AndAssignScalar)
{
    UVector2 vector = UVector2(3u, 7u);
    vector &= 3u;

    BOOST_CHECK_EQUAL(vector, UVector2(3u, 3u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_AndAssignVector)
{
    UVector2 vector = UVector2(9u, 3u);
    vector &= UVector2(3u, 9u);

    BOOST_CHECK_EQUAL (vector, UVector2(1u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_OrScalar)
{
    UVector2 vector = UVector2(2u, 4u);
    UVector2 result = vector | 2u;

    BOOST_CHECK_EQUAL(result, UVector2(2u, 6u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_OrVector)
{
    UVector2 vector = UVector2(4u, 1u);
    UVector2 result = vector | UVector2(1u, 1u);

    BOOST_CHECK_EQUAL(result, UVector2(5u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_OrAssignScalar)
{
    UVector2 vector = UVector2(3u, 7u);
    vector |= 3u;

    BOOST_CHECK_EQUAL(vector, UVector2(3u, 7u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_OrAssignVector)
{
    UVector2 vector = UVector2(9u, 3u);
    vector |= UVector2(3u, 9u);

    BOOST_CHECK_EQUAL (vector, UVector2(11u, 11u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_XorScalar)
{
    UVector2 vector = UVector2(2u, 4u);
    UVector2 result = vector ^ 2u;

    BOOST_CHECK_EQUAL(result, UVector2(0u, 6u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_XorVector)
{
    UVector2 vector = UVector2(4u, 1u);
    UVector2 result = vector ^ UVector2(1u, 1u);

    BOOST_CHECK_EQUAL(result, UVector2(5u, 0u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_XorAssignScalar)
{
    UVector2 vector = UVector2(3u, 7u);
    vector ^= 3u;

    BOOST_CHECK_EQUAL(vector, UVector2(0u, 4u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_XorAssignVector)
{
    UVector2 vector = UVector2(9, 3);
    vector ^= UVector2(3, 9);

    BOOST_CHECK_EQUAL (vector, UVector2(10, 10));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_RemainderScalar)
{
    UVector2 vector = UVector2(15u, 11u);
    UVector2 result = vector % 10u;

    BOOST_CHECK_EQUAL(result, UVector2(5u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_RemainderVector)
{
    UVector2 vector = UVector2(5u, 7u);
    UVector2 result = vector % UVector2(5u, 3u);

    BOOST_CHECK_EQUAL(result, UVector2(0u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_RemainderAssignScalar)
{
    UVector2 vector = UVector2(15u, 11u);
    vector %= 10u;

    BOOST_CHECK_EQUAL(vector, UVector2(5u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_RemainderAssignVector)
{
    UVector2 vector = UVector2(5u, 7u);
    vector %= UVector2(5u, 3u);

    BOOST_CHECK_EQUAL(vector, UVector2(0u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_Not)
{
    IVector2 vector = IVector2(14, 5);
    IVector2 result = ~vector;

    BOOST_CHECK_EQUAL(result, IVector2(-15, -6));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftLeftScalar)
{
    UVector2 vector = UVector2(2u, 3u);
    UVector2 result = vector << 4u;

    BOOST_CHECK_EQUAL(result, UVector2(32u, 48u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftLeftVector)
{
    UVector2 vector = UVector2(4u, 5u);
    UVector2 result = vector << UVector2(1u, 2u);

    BOOST_CHECK_EQUAL(result, UVector2(8u, 20u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftLeftAssignScalar)
{
    UVector2 vector = UVector2(2u, 3u);
    vector <<= 4u;

    BOOST_CHECK_EQUAL(vector, UVector2(32u, 48u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftLeftAssignVector)
{
    UVector2 vector = UVector2(4u, 5u);
    vector <<= UVector2(1u, 2u);

    BOOST_CHECK_EQUAL(vector, UVector2(8u, 20u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftRightScalar)
{
    UVector2 vector = UVector2(2u, 10u);
    UVector2 result = vector >> 3u;

    BOOST_CHECK_EQUAL(result, UVector2(0u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftRightVector)
{
    UVector2 vector = UVector2(17u, 26u);
    UVector2 result = vector >> UVector2(4u, 2u);

    BOOST_CHECK_EQUAL(result, UVector2(1u, 6u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftRightAssignScalar)
{
    UVector2 vector = UVector2(2u, 10u);
    vector >>= 3u;

    BOOST_CHECK_EQUAL(vector, UVector2(0u, 1u));
}

BOOST_AUTO_TEST_CASE(Bit_Vector2_ShiftRightAssignVector)
{
    UVector2 vector = UVector2(17u, 26u);
    vector >>= UVector2(4u, 2u);

    BOOST_CHECK_EQUAL(vector, UVector2(1u, 6u));
}


/*
 * external_rotation_estimator.cc
 *
 *  Created on: Jul 8, 2016
 *      Author: LeonMing
 */

#include "theia/sfm/global_pose_estimation/external_rotation_estimator.h"

#include <ceres/rotation.h>
#include <ceres/ceres.h>
#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <Eigen/SparseQR>
#include <unordered_map>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>

#include "theia/sfm/pose/util.h"
#include "theia/sfm/twoview_info.h"
#include "theia/sfm/types.h"
#include "theia/util/hash.h"
#include "theia/util/map_util.h"

namespace theia {
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::MatrixXd;

bool ExternalRotationEstimator::EstimateRotations(
	const std::unordered_map<ViewIdPair, TwoViewInfo>& view_pairs,
	std::unordered_map<ViewId, Eigen::Vector3d>* orientations) {

	// Read in the global rotations file
	std::ifstream ifs(global_rotations_file_.c_str(), std::ios::in);
	if (!ifs.is_open()) {
		LOG(ERROR) << "Cannot read the global rotations file from " << global_rotations_file_;
	    return false;
	}
	// Loop through the file. For every ten numbers, the first number is a view_id
	// and the next nine numbers are a rotation matrix
	while (!ifs.eof()) {

		ViewId view_id;
		ifs >> view_id;

		// Add rotation matrix values to a Matrix3d
		Eigen::Matrix3d rotation;

        for (int i = 0; i < 3; ++i) {
        	for (int j = 0; j < 3; ++j) {
        		ifs >> rotation(j, i);
        	}
		}

        // Declare an angle-axis Vector3d
		Eigen::Vector3d angleaxis;

		// Convert rotation matrix values to angle-axis vector
		ceres::RotationMatrixToAngleAxis(rotation.data(), angleaxis.data());

		// Place corresponding view ids and angle-axis vectors in orientations
		orientations->emplace(view_id, angleaxis);
	}
return true;
}

}  // namespace theia


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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_TRIGONOMETRIC_CONSTANTS_DEGINRAD_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_DEGINRAD_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
   /*!
     @brief Deginrad generic tag

     Represents the Deginrad constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Deginrad, double
                                , 0, 0x3c8efa35
                                , 0x3f91df46a2529d39ll
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Deginrad, Site> dispatching_Deginrad(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Deginrad, Site>();
   }
   template<class... Args>
   struct impl_Deginrad;
  }
  /*!
    Constant radian in Degree multiplier, \f$\frac{180}\pi\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = Deginrad<T0>();
    @endcode

    is similar to:

    @code
    T0 r = _180<T0>()/Pi<T0>() ;
    @endcode

    @see  @funcref{inrad}, @funcref{indeg}, @funcref{Radindeg}, @funcref{Radindegr}
    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Deginrad, Deginrad);

}

#endif


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef AUTODIF_HPP
#define AUTODIF_HPP

// Automatic Differentiation by bottom up algorithm

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <cmath>

#include <kv/convert.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> class autodif;
template <class C, class T> struct convertible<C, autodif<T> > {
	static const bool value = convertible<C, T>::value || boost::is_same<C, autodif<T> >::value;
};
template <class C, class T> struct acceptable_n<C, autodif<T> > {
	static const bool value = convertible<C, T>::value;
};

template <class T> class autodif {
	public:
	T v;
	ub::vector<T> d;

	typedef T base_type;

	autodif() {
		v = 0.;
		d.resize(0);
	}

	template <class C> explicit autodif(const C& x, typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value >::type* =0) {
		v = x;
		d.resize(0);
	}

	template <class C> typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator=(const C& x) {
		v = x;
		d.resize(0);
		return *this;
	}

	friend autodif operator+(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v + b.v;

		if (a.d.size() == 0) {
			r.d = b.d;
		} else if (b.d.size() == 0) {
			r.d = a.d;
		} else {
			r.d = a.d + b.d;
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator+(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v + b;
		r.d = a.d;

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator+(const C& a, const autodif& b) {
		autodif r;

		r.v = a + b.v;
		r.d = b.d;

		return r;
	}

	friend autodif& operator+=(autodif& a, const autodif& b) {
		a = a + b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator+=(autodif& a, const C& b) {
		a.v += b;
		return a;
	}

	friend autodif operator-(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v - b.v;

		if (a.d.size() == 0) {
			r.d = - b.d;
		} else if (b.d.size() == 0) {
			r.d = a.d;
		} else {
			r.d = a.d - b.d;
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator-(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v - b;
		r.d = a.d;

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator-(const C& a, const autodif& b) {
		autodif r;

		r.v = a - b.v;
		r.d = - b.d;

		return r;
	}

	friend autodif& operator-=(autodif& a, const autodif& b) {
		a = a - b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator-=(autodif& a, const C& b) {
		a.v -= b;
		return a;
	}

	friend autodif operator-(const autodif& a) {
		autodif r;

		r.v = - a.v;
		r.d = - a.d;

		return r;
	}

	friend autodif operator*(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v * b.v;

		if (a.d.size() == 0) {
			r.d = a.v * b.d;
		} else if (b.d.size() == 0) {
			r.d = b.v * a.d;
		} else {
			r.d = b.v * a.d + a.v * b.d;
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator*(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v * b;
		// r.d = b * a.d;
		r.d = T(b) * a.d; // assist for VC++

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator*(const C& a, const autodif& b) {
		autodif r;

		r.v = a * b.v;
		// r.d = a * b.d;
		r.d = T(a) * b.d; // assist for VC++

		return r;
	}

	friend autodif& operator*=(autodif& a, const autodif& b) {
		a = a * b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator*=(autodif& a, const C& b) {
		a.v *= b;
		// a.d *= b;
		a.d *= T(b); // assist for VC++
		return a;
	}

	friend autodif operator/(const autodif& a, const autodif& b) {
		autodif r;

		r.v = a.v / b.v;

		if (a.d.size() == 0) {
			r.d = b.d * (-a.v/(b.v*b.v));
		} else if (b.d.size() == 0) {
			r.d = a.d / b.v;
		} else {
			r.d = a.d / b.v + b.d * (-a.v/(b.v*b.v));
		}

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator/(const autodif& a, const C& b) {
		autodif r;

		r.v = a.v / b;
		// r.d = a.d / b;
		r.d = a.d / T(b); // assist for VC++

		return r;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type operator/(const C& a, const autodif& b) {
		autodif r;

		r.v = a / b.v;
		r.d = b.d * (-a/(b.v*b.v));

		return r;
	}

	friend autodif& operator/=(autodif& a, const autodif& b) {
		a = a / b;
		return a;
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif& >::type operator/=(autodif& a, const C& b) {
		a.v /= b;
		// a.d /= b;
		a.d /= T(b); // assist for VC++
		return a;
	}

	friend std::ostream& operator<<(std::ostream& s, const autodif& x) {
		int i;
		int n = x.d.size();
		s << x.v;
		s << '<';
		for (i=0; i<n; i++) {
			s << x.d(i);
			if (i != n-1) {
				s << ',';
			}
		}
		s << '>';
		return s;
	}

	friend autodif pow(const autodif& x, int y) {
		autodif r;

		using std::pow;
		r.v = pow(x.v, y);
		if (y == 0) {
			r.d = T(0.) * x.d;
		} else {
			r.d = (y * pow(x.v, y - 1)) * x.d;
		}
		return r;
	}

	friend autodif pow(const autodif& x, const autodif& y) {
		return exp(y * log(x));
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value && ! boost::is_integral<C>::value, autodif >::type pow(const autodif& a, const C& b) {
		return pow(a, autodif(b));
	}

	template <class C> friend typename boost::enable_if_c< kv::acceptable_n<C, autodif>::value, autodif >::type pow(const C& a, const autodif& b) {
		return pow(autodif(a), b);
	}

	friend autodif exp (const autodif& x) {
		autodif r;

		using std::exp;
		r.v = exp(x.v);
		// r.d = exp(x.v) * x.d;
		r.d = r.v * x.d;

		return r;
	}

	friend autodif log (const autodif& x) {
		autodif r;

		using std::log;
		r.v = log(x.v);
		r.d = x.d / x.v;

		return r;
	}

	friend autodif sqrt (const autodif& x) {
		autodif r;

		using std::sqrt;
		r.v = sqrt(x.v);
		// r.d = 1./(2. * sqrt(x.v)) * x.d;
		r.d = x.d / (2. * r.v);

		return r;
	}

	friend autodif sin (const autodif& x) {
		autodif r;

		using std::sin;
		using std::cos;
		r.v = sin(x.v);
		r.d = cos(x.v) * x.d;

		return r;
	}

	friend autodif cos (const autodif& x) {
		autodif r;

		using std::sin;
		using std::cos;
		r.v = cos(x.v);
		r.d = -sin(x.v) * x.d;

		return r;
	}

	friend autodif tan (const autodif& x) {
		autodif r;
		T tmp;

		using std::tan;
		using std::cos;
		r.v = tan(x.v);
		tmp = cos(x.v);
		tmp = 1. / (tmp * tmp);
		r.d = tmp * x.d;

		return r;
	}

	friend autodif asin (const autodif& x) {
		autodif r, tmp;

		using std::asin;
		using std::sqrt;
		r.v = asin(x.v);
		r.d = (1. / sqrt(1. - x.v * x.v)) * x.d;

		return r;
	}

	friend autodif acos (const autodif& x) {
		autodif r;

		using std::acos;
		using std::sqrt;
		r.v = acos(x.v);
		r.d = (-1. / sqrt(1. - x.v * x.v)) * x.d;

		return r;
	}

	friend autodif atan (const autodif& x) {
		autodif r;

		using std::atan;
		r.v = atan(x.v);
		r.d = (1. / (1. + x.v * x.v)) * x.d;

		return r;
	}

	friend autodif sinh (const autodif& x) {
		autodif r;

		using std::sinh;
		using std::cosh;
		r.v = sinh(x.v);
		r.d = cosh(x.v) * x.d;

		return r;
	}

	friend autodif cosh (const autodif& x) {
		autodif r;

		using std::sinh;
		using std::cosh;
		r.v = cosh(x.v);
		r.d = sinh(x.v) * x.d;

		return r;
	}

	friend autodif tanh (const autodif& x) {
		autodif r;
		T tmp;

		using std::tanh;
		using std::cosh;
		r.v = tanh(x.v);
		tmp = cosh(x.v);
		tmp = 1. / (tmp * tmp);
		r.d = tmp * x.d;

		return r;
	}

	friend autodif asinh (const autodif& x) {
		autodif r, tmp;

		// using std::asinh;
		using std::sqrt;
		r.v = asinh(x.v);
		r.d = (1. / sqrt(x.v * x.v + 1.)) * x.d;

		return r;
	}

	friend autodif acosh (const autodif& x) {
		autodif r;

		// using std::acosh;
		using std::sqrt;
		r.v = acosh(x.v);
		r.d = (1. / sqrt(x.v * x.v - 1.)) * x.d;

		return r;
	}

	friend autodif atanh (const autodif& x) {
		autodif r;

		// using std::atanh;
		r.v = atanh(x.v);
		r.d = (1. / (1. - x.v * x.v)) * x.d;

		return r;
	}

	// n-dimensional version
	static ub::vector<autodif> init (const ub::vector<T>& in) {
		int i, j;
		int n = in.size();
		ub::vector<autodif> out(n);

		for (i=0; i<n; i++) {
			out(i).v = in(i);
			out(i).d.resize(n);
			for (j=0; j<n; j++) {
				out(i).d(j) = (i==j) ? 1. : 0.;
			}
		}

		return out;
	}

	// 1-dimensional version
	static autodif init (const T& in) {
		int j;
		autodif out;

		out.v = in;
		out.d.resize(1);
		out.d(0) = 1.;

		return out;
	}

	// for functions R^n -> R^m
	static void split (const ub::vector<autodif>& in, ub::vector<T>& v, ub::matrix<T>& d) {
		int i, j, n, tmp;
		int m = in.size();

		if (in.size() == 0) return;
		n = in(0).d.size();
		for (i=1; i<m; i++) {
			tmp = in(i).d.size();
			if (tmp > n) n = tmp;
		}

		v.resize(m);
		d.resize(m, n);
		for (i=0; i<m; i++) {
			v(i) = in(i).v;
			tmp = in(i).d.size();
			for (j=0; j<tmp; j++) {
				d(i, j) = in(i).d(j);
			}
			for (j=tmp; j<n; j++) {
				d(i, j) = 0.;
			}
		}
	}

	// for functions R^n -> R
	static void split (const autodif& in, T& v, ub::vector<T>& d) {
		int j, n;

		n = in.d.size();
		d.resize(n);

		v = in.v;
		for (j=0; j<n; j++) {
			d(j) = in.d(j);
		}
	}

	// for functions R -> R^m
	static void split (const ub::vector<autodif>& in, ub::vector<T>& v, ub::vector<T>& d) {
		int i;
		int m = in.size();

		if (in.size() == 0) return;

		v.resize(m);
		d.resize(m);
		for (i=0; i<m; i++) {
			v(i) = in(i).v;
			d(i) = in(i).d(0);
		}
	}

	// for functions R -> R
	static void split (const autodif& in, T& v, T& d) {
		v = in.v;
		d = in.d(0);
	}

	static ub::vector<autodif>
	compress (const ub::vector<autodif>& in, ub::matrix<T>& save) {
		ub::vector<autodif> out;
		int i, j, m, tmp;
		int n = in.size();

		out.resize(n);

		m = in(0).d.size();
		for (i=1; i<n; i++) {
			tmp = in(i).d.size();
			if (tmp > m) m = tmp;
		}

		save.resize(n, m);

		for (i=0; i<n; i++) {
			out(i).v = in(i).v;
			tmp = in(i).d.size();
			for (j=0; j<tmp; j++) {
				save(i, j) = in(i).d(j);
			}
			for (j=tmp; j<m; j++) {
				save(i, j) = 0.;
			}
			out(i).d.resize(n);
			for (j=0; j<n; j++) {
				out(i).d(j) = (i==j) ? 1 : 0;
			}
		}

		return out;
	}

	static autodif
	expand (const autodif& in, const ub::matrix<T>& save) {
		autodif out;

		out.v = in.v;
		out.d = prod(in.d, save);

		return out;
	}

	static ub::vector<autodif>
	expand (const ub::vector<autodif>& in, const ub::matrix<T>& save) {
		ub::vector<autodif> out;
		int i;
		int s = in.size();

		out.resize(s);

		for (i=0; i<s; i++) {
			out(i) = expand(in(i), save);
		}

		return out;
	}
};

} // namespace kv

#endif //AUTODIF_HPP


#define BOOST_TEST_MODULE config
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

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

#include "flame/util.h"

BOOST_AUTO_TEST_CASE(parse_table_empty)
{
    std::istringstream strm;

    numeric_table tbl;
    tbl.read(strm);

    BOOST_CHECK_EQUAL(tbl.table.size1(), 0);
    BOOST_CHECK_EQUAL(tbl.table.size2(), 0);
    BOOST_CHECK_EQUAL(tbl.colnames.size(), 0);
}

static const char table1[] = {
    "%A B\n"
    " 12 13\n"
    "14   15 \n"
    " 16 17\n"
};

BOOST_AUTO_TEST_CASE(parse_table1)
{
    std::istringstream strm(table1);

    numeric_table tbl;
    tbl.read(strm);

    BOOST_CHECK_EQUAL(tbl.table.size1(), 3);
    BOOST_CHECK_EQUAL(tbl.table.size2(), 2);
    BOOST_CHECK_EQUAL(tbl.colnames.size(), 2);

    BOOST_CHECK_EQUAL(tbl.colnames["A"], 0);
    BOOST_CHECK_EQUAL(tbl.colnames["B"], 1);

    std::ostringstream out;
    out<<tbl.table;

    BOOST_CHECK_EQUAL(out.str(), "[3,2]((12,13),(14,15),(16,17))");
}

static const char table2[] = {
    // no column names
    " 12 13\n"
    "14   15\n"
    " 16 17"  // no trailing newline
};

BOOST_AUTO_TEST_CASE(parse_table2)
{
    std::istringstream strm(table2);

    numeric_table tbl;
    tbl.read(strm);

    BOOST_CHECK_EQUAL(tbl.table.size1(), 3);
    BOOST_CHECK_EQUAL(tbl.table.size2(), 2);
    BOOST_CHECK_EQUAL(tbl.colnames.size(), 0);

    std::ostringstream out;
    out<<tbl.table;

    BOOST_CHECK_EQUAL(out.str(), "[3,2]((12,13),(14,15),(16,17))");
}

static const char table3[] = {
    "%A B\n"
    " 12 13\n"
    "14   15 16 17\n"  // wrong number of columns
};

BOOST_AUTO_TEST_CASE(parse_table3)
{
    std::istringstream strm(table3);

    numeric_table tbl;
    BOOST_CHECK_THROW(tbl.read(strm), std::runtime_error);
}

static const char table4[] = {
    "%A B\n"
    " 12 13\n"
    "14   hello\n" // not numeric
    "16 17\n"
};

BOOST_AUTO_TEST_CASE(parse_table4)
{
    std::istringstream strm(table4);

    numeric_table tbl;
    BOOST_CHECK_THROW(tbl.read(strm), std::runtime_error);
}

BOOST_AUTO_TEST_CASE(test_ndindex_iterate)
{
    size_t limits[2] = {2,3};
    ndindex_iterate<2> iter(2, limits);

    BOOST_CHECK(!iter.done);
    BOOST_CHECK_EQUAL(iter.ndim, 2);
    BOOST_CHECK_EQUAL_COLLECTIONS(limits, limits+2, iter.limit, iter.limit+2);

#define CHECKME(isdone, I, J) {size_t P[2] = {I,J}; BOOST_CHECK_EQUAL(isdone, iter.done); \
    BOOST_CHECK_EQUAL_COLLECTIONS(P, P+2, iter.index, iter.index+2); }

    CHECKME(false, 0, 0);
    iter.next();
    CHECKME(false, 1, 0);
    iter.next();
    CHECKME(false, 0, 1);
    iter.next();
    CHECKME(false, 1, 1);
    iter.next();
    CHECKME(false, 0, 2);
    iter.next();
    CHECKME(false, 1, 2);
    iter.next();
    CHECKME(true,  0, 0);

#undef CHECKME
}


#ifndef N_BODY_DATA_HPP
#define N_BODY_DATA_HPP

#include <array>
#include <boost/serialization/access.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
#include <cmath>
#include <cstddef>
#include <memory>
#include <tuple>
#include <vector>

namespace n_body::data {

template <typename T, std::size_t Dimension>
using Vector = std::array<T, Dimension>;

template <typename T> using Scalar = T;

template <typename T, std::size_t Dimension> struct Body {
  using vector_type = Vector<T, Dimension>;
  using scalar_type = Scalar<T>;

  vector_type position;
  vector_type velocity;
  scalar_type mass;

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(position);
    ar &BOOST_SERIALIZATION_NVP(velocity);
    ar &BOOST_SERIALIZATION_NVP(mass);
  }
};

template <typename T, std::size_t Dimension>
using Bodies = std::vector<Body<T, Dimension>>;

template <typename T, std::size_t Dimension> struct Space {
  using vector_type = Vector<T, Dimension>;

  vector_type min;
  vector_type max;
  vector_type center;

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(min);
    ar &BOOST_SERIALIZATION_NVP(max);
    ar &BOOST_SERIALIZATION_NVP(center);
  }
};

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator+(const Vector<T, Dimension> &v1,
                               const Vector<T, Dimension> &v2) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v1[d] + v2[d];
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator+=(Vector<T, Dimension> &v1,
                                 const Vector<T, Dimension> &v2) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v1[d] += v2[d];
  }
  return v1;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator-(const Vector<T, Dimension> &v1,
                               const Vector<T, Dimension> &v2) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v1[d] - v2[d];
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator-(const Vector<T, Dimension> &v) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = -v[d];
  }
  return result;
}

template <typename T, std::size_t Dimension>
Scalar<T> module_of(const Vector<T, Dimension> &v) {
  T sum = 0;
  for (auto x : v) {
    sum += x * x;
  }
  return std::sqrt(sum);
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator-=(Vector<T, Dimension> &v1,
                                 const Vector<T, Dimension> &v2) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v1[d] -= v2[d];
  }
  return v1;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator*(const Scalar<T> &s,
                               const Vector<T, Dimension> &v) {
  Vector<T, Dimension> result{};
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v[d] * s;
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator*(const Vector<T, Dimension> &v,
                               const Scalar<T> &s) {
  return s * v;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator*=(Vector<T, Dimension> &v, const Scalar<T> &s) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v[d] *= s;
  }
  return v;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> operator/(const Vector<T, Dimension> &v,
                               const Scalar<T> &s) {
  Vector<T, Dimension> result;
  for (std::size_t d = 0; d < Dimension; ++d) {
    result[d] = v[d] / s;
  }
  return result;
}

template <typename T, std::size_t Dimension>
Vector<T, Dimension> &operator/=(Vector<T, Dimension> &v, const Scalar<T> &s) {
  for (std::size_t d = 0; d < Dimension; ++d) {
    v[d] /= s;
  }
  return v;
}

template <typename T, std::size_t Dimension>
std::tuple<Vector<T, Dimension>, Scalar<T>> average_position_by_mass(
    const Vector<T, Dimension> &position1, const Scalar<T> &mass1,
    const Vector<T, Dimension> &position2, const Scalar<T> &mass2) {
  auto sum = mass1 * position1 + mass2 * position2;
  auto sum_mass = mass1 + mass2;
  return {
      sum / sum_mass,
      sum_mass,
  };
}

template <typename T, std::size_t Dimension>
void average_position_by_mass_in_place(Vector<T, Dimension> &position1,
                                       Scalar<T> &mass1,
                                       const Vector<T, Dimension> &position2,
                                       const Scalar<T> &mass2) {
  position1 *= mass1;
  position1 += position2 * mass2;
  mass1 += mass2;
  position1 /= mass1;
}

} // namespace n_body::data

#endif


#ifndef NNQS_OPTIMIZERS_SGD_HPP
#define NNQS_OPTIMIZERS_SGD_HPP
#include <Eigen/Dense>
#include <nlohmann/json.hpp>
#include "Optimizers/Optimizer.hpp"

namespace yannq
{
template<typename T>
class SGD
	: public Optimizer<T>
{
public:
	using typename Optimizer<T>::Vector;
	using typename Optimizer<T>::RealVector;

	static constexpr double DEFAULT_PARAMS[] = {0.05, 0.0};

private:
	double alpha_;
	double p_;
	int t_;

public:

	SGD(double alpha = DEFAULT_PARAMS[0], double p = DEFAULT_PARAMS[1])
		: alpha_{alpha}, p_{p}, t_{0}
	{
	}

	SGD(const nlohmann::json& params)
		: alpha_{params.value("alpha", DEFAULT_PARAMS[0])},
			p_{params.value("p", DEFAULT_PARAMS[1])},
			t_{0}
	{
	}

	nlohmann::json desc() const override
	{
		return nlohmann::json
		{
			{"name", "SGD"},
			{"alhpa", alpha_},
			{"p", p_}
		};
	}

	Vector getUpdate(const Vector& v) override
	{
		using std::pow;
		++t_;
		double eta = std::max((alpha_/pow(t_, p_)), 1e-4);
		return -eta*v;
	}
};
}//namespace yannq
template<typename T>
constexpr double yannq::SGD<T>::DEFAULT_PARAMS[];
#endif//NNQS_OPTIMIZERS_SGD_HPP


/******************************************************************************
Copyright (c) 2021, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder 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 HOLDER 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.
******************************************************************************/

#include "ocs2_core/loopshaping/LoopshapingPropertyTree.h"

#include <boost/property_tree/info_parser.hpp>
#include "ocs2_core/misc/LoadData.h"

namespace ocs2 {
namespace loopshaping_property_tree {
Filter readSISOFilter(const boost::property_tree::ptree& pt, std::string filterName, bool invert) {
  // Get Sizes
  const auto numRepeats = pt.get<size_t>(filterName + ".numRepeats", 1);
  const auto numPoles = pt.get<size_t>(filterName + ".numPoles", 0);
  const auto numZeros = pt.get<size_t>(filterName + ".numZeros", 0);
  const auto scaling = pt.get<scalar_t>(filterName + ".scaling", 1.0);
  const size_t numStates = invert ? numZeros : numPoles;
  const size_t numInputs = 1;

  // Setup Filter, convention a0*s^n + a1*s^(n-1) + ... + an
  vector_t numerator(numZeros + 1);
  numerator.setZero();
  numerator(0) = 1.0;
  for (size_t z = 0; z < numZeros; z++) {
    auto zero = pt.get<scalar_t>(filterName + ".zeros." + "(" + std::to_string(z) + ")");
    numerator.segment(1, z + 1) -= zero * numerator.segment(0, z + 1).eval();
  }
  numerator *= scaling;  // Apply scale to numerator

  vector_t denominator(numPoles + 1);
  denominator.setZero();
  denominator(0) = 1.0;
  for (size_t p = 0; p < numPoles; p++) {
    auto pole = pt.get<scalar_t>(filterName + ".poles." + "(" + std::to_string(p) + ")");
    denominator.segment(1, p + 1) -= pole * denominator.segment(0, p + 1).eval();
  }

  // Print frequency domain information
  std::cerr << "Read filter " << filterName << (invert ? " (before inversion)" : "") << ", convention: a0*s^n + a1*s^(n-1) + ... + an \n";
  std::cerr << "\tnumerator: [" << numerator.transpose() << "]\n";
  std::cerr << "\tdenominator: [" << denominator.transpose() << "]\n";
  std::cerr << "\tDC gain: " << numerator(numZeros) / denominator(numPoles) << "\n";
  std::cerr << "\tInf gain: ";
  if (numZeros > numPoles) {
    std::cerr << "Inf\n";
  }
  if (numZeros < numPoles) {
    std::cerr << 0.0 << "\n";
  }
  if (numZeros == numPoles) {
    std::cerr << numerator(0) / denominator(0) << "\n";
  }

  if (invert) {
    vector_t temp;
    temp = numerator;
    numerator = denominator;
    denominator = temp;
  }

  // Convert to state space
  matrix_t a, b, c, d;
  ocs2::tf2ss(numerator, denominator, a, b, c, d);

  matrix_t A = matrix_t::Zero(numRepeats * numStates, numRepeats * numStates);
  matrix_t B = matrix_t::Zero(numRepeats * numStates, numRepeats * numInputs);
  matrix_t C = matrix_t::Zero(numRepeats * numInputs, numRepeats * numStates);
  matrix_t D = matrix_t::Zero(numRepeats * numInputs, numRepeats * numInputs);
  for (size_t r = 0; r < numRepeats; r++) {
    A.block(r * numStates, r * numStates, numStates, numStates) = a;
    B.block(r * numStates, r * numInputs, numStates, numInputs) = b;
    C.block(r * numInputs, r * numStates, numInputs, numStates) = c;
    D.block(r * numInputs, r * numInputs, numInputs, numInputs) = d;
  }

  return Filter(A, B, C, D);
}

Filter readMIMOFilter(const boost::property_tree::ptree& pt, std::string filterName, bool invert) {
  const auto numFilters = pt.get<size_t>(filterName + ".numFilters", 0);
  if (numFilters > 0) {
    // Read the sisoFilters
    std::vector<Filter> sisoFilters;
    size_t numStates(0), numInputs(0), numOutputs(0);
    for (size_t i = 0; i < numFilters; ++i) {
      // Read filter
      std::string sisoFilterName = filterName + ".Filter" + std::to_string(i);
      sisoFilters.emplace_back(readSISOFilter(pt, sisoFilterName, invert));

      // Track sizes
      numStates += sisoFilters.back().getNumStates();
      numInputs += sisoFilters.back().getNumInputs();
      numOutputs += sisoFilters.back().getNumOutputs();
    }

    // Concatenate siso matrices into one MIMO filter
    matrix_t A = matrix_t::Zero(numStates, numStates);
    matrix_t B = matrix_t::Zero(numStates, numInputs);
    matrix_t C = matrix_t::Zero(numOutputs, numStates);
    matrix_t D = matrix_t::Zero(numOutputs, numInputs);
    size_t statecount(0), inputcount(0), outputcount(0);
    for (const auto& filt : sisoFilters) {
      A.block(statecount, statecount, filt.getNumStates(), filt.getNumStates()) = filt.getA();
      B.block(statecount, inputcount, filt.getNumStates(), filt.getNumInputs()) = filt.getB();
      C.block(outputcount, statecount, filt.getNumOutputs(), filt.getNumStates()) = filt.getC();
      D.block(outputcount, inputcount, filt.getNumOutputs(), filt.getNumInputs()) = filt.getD();
      statecount += filt.getNumStates();
      inputcount += filt.getNumInputs();
      outputcount += filt.getNumOutputs();
    }
    return Filter(A, B, C, D);
  } else {
    return Filter();
  }
}

std::shared_ptr<LoopshapingDefinition> load(const std::string& settingsFile) {
  // Read from settings File
  boost::property_tree::ptree pt;
  boost::property_tree::read_info(settingsFile, pt);
  Filter r_filter = loopshaping_property_tree::readMIMOFilter(pt, "r_filter");
  Filter s_filter = loopshaping_property_tree::readMIMOFilter(pt, "s_inv_filter", /*invert=*/true);

  if (r_filter.getNumOutputs() > 0 && s_filter.getNumOutputs() > 0) {
    throw std::runtime_error("[LoopshapingDefinition] using both r and s filter not implemented");
  }

  if (r_filter.getNumOutputs() > 0) {
    return std::make_shared<LoopshapingDefinition>(LoopshapingType::outputpattern, r_filter);
  }
  if (s_filter.getNumOutputs() > 0) {
    return std::make_shared<LoopshapingDefinition>(LoopshapingType::eliminatepattern, s_filter);
  }

  throw std::runtime_error("[LoopshapingDefinition] error loading loopshaping definition, no valid filter found");
}

}  // namespace loopshaping_property_tree
}  // namespace ocs2


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/oneosqrt5.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check oneosqrt5 behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::oneosqrt5;
  using boost::simd::Oneosqrt5;

  STF_TYPE_IS(decltype(Oneosqrt5<T>()), T);
  STF_EQUAL(Oneosqrt5<T>(), T(0));
  STF_EQUAL(oneosqrt5( as(T{}) ),T(0));
}

STF_CASE_TPL( "Check oneosqrt5 behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::oneosqrt5;
  using boost::simd::Oneosqrt5;

  STF_TYPE_IS(decltype(Oneosqrt5<T>()), T);
  auto z1 = Oneosqrt5<T>();
  STF_ULP_EQUAL(z1*z1, T(1/5.0), 0.5);
  auto z2 = oneosqrt5( as(T{}) );
  STF_ULP_EQUAL(z2*z2, T(1/5.0), 0.5);
}


#include <Core/Core.h>

#include <Eigen/Eigen.h>
#include <Surface/Surface.h>
#include <Geom/Geom.h>
#include <Functions4U/Functions4U.h>

namespace Upp {
using namespace Eigen;

static int IsTabSpace(int c) {
	if (c == '\t' || c == ' ' || c == '!')
		return true;
	return false;
}

static void LoadStlTxt(String fileName, Surface &surf, bool &isText) {
	isText = false;

	FileInLine in(fileName);
	if (!in.IsOpen())
		throw Exc(Format(t_("Impossible to open file '%s'"), fileName));

	String line;
	FieldSplit f(in);
	f.IsSeparator = IsTabSpace;

	line = ToLower(TrimBoth(in.GetLine()));
	if (!line.StartsWith("solid"))
		throw Exc(in.Str() + ". "  + t_("'solid' field not found"));

	isText = true;

    while (!in.IsEof()) {
		line = ToLower(TrimBoth(in.GetLine()));
		if (line.IsEmpty())
			continue;

		if (line.StartsWith("endsolid"))
			break;

		if (!line.StartsWith("facet normal"))
			throw Exc(in.Str() + ". "  + t_("'facet normal' field not found"));

		line = ToLower(TrimBoth(in.GetLine()));
		if (!line.StartsWith("outer loop"))
			throw Exc(in.Str() + ". "  + t_("'outer loop' field not found"));

		int ids[5];
		for (int i = 0; i < 6; ++i) {
			f.Load(in.GetLine());
			String label = f.GetText(0);

			if (label == "vertex") {
				if (i == 4)
					throw Exc(in.Str() + ". "  + t_("Too much vertex in facet"));
				Point3D node(f.GetDouble(1), f.GetDouble(2), f.GetDouble(3));
				surf.nodes << node;
				ids[i] = surf.nodes.GetCount() - 1;
			} else if (label == "endloop") {
				if (i < 3)
					throw Exc(in.Str() + ". "  + t_("Too few vertex in facet"));
				Panel &panel = surf.panels.Add();
				panel.id[0] = ids[0];
				panel.id[1] = ids[1];
				panel.id[2] = ids[2];
				if (i == 3)
					panel.id[3] = ids[0];
				else if (i == 4)
					panel.id[3] = ids[3];
			} else if (label == "endfacet")
				break;
			else
				throw Exc(in.Str() + ". "  + Format(t_("Label '%s' not handled in facet"), label));
		}
	}
}

static void STLFacetTxtOut(FileOut &out, const Point3D &p0, const Point3D &p1, const Point3D &p2,
						const Point3D &p3, const Vector3D &normal) {
	out << "facet normal " << normal.x << " " << normal.y << " " << normal.z << "\n";
	out << "   outer loop" << "\n";
	out << "      vertex " << p0.x << " " << p0.y << " " << p0.z << "\n";
	out << "      vertex " << p1.x << " " << p1.y << " " << p1.z << "\n";
	out << "      vertex " << p2.x << " " << p2.y << " " << p2.z << "\n";
	if (!IsNull(p3))
		out << "      vertex " << p3.x << " " << p3.y << " " << p3.z << "\n";
	out << "   endloop" << "\n";
	out << "endfacet" << "\n";
}

void SaveStlTxt(String fileName, const Surface &surf) {
	FileOut out(fileName);
	if (!out.IsOpen())
		throw Exc(Format(t_("Impossible to open '%s'\n"), fileName));

	const Vector<Panel> &panels = surf.panels;
	const Vector<Point3D> &nodes = surf.nodes;

	bool forceTriangles = true;

	out << "solid U++ STL mesh export" << "\n";

	for (int i = 0; i < panels.GetCount(); ++i) {
		const Panel &panel = panels[i];
		const Point3D &p0 = nodes[panel.id[0]];
		const Point3D &p1 = nodes[panel.id[1]];
		const Point3D &p2 = nodes[panel.id[2]];
		if (forceTriangles) {
			STLFacetTxtOut(out, p0, p1, p2, Null, panel.normal0);
			if (!panel.IsTriangle()) {
				const Point3D &p3 = nodes[panel.id[3]];
				STLFacetTxtOut(out, p2, p3, p0, Null, panel.normal1);
			}
		} else {
			if (panel.IsTriangle())
				STLFacetTxtOut(out, p0, p1, p2, Null, panel.normalPaint);
			else {
				const Point3D &p3 = nodes[panel.id[3]];
				STLFacetTxtOut(out, p0, p1, p2, p3, panel.normalPaint);
			}
		}
	}
	out << "endsolid U++ STL mesh export" << "\n";
}

static void LoadStlBin(String fileName, Surface &surf, String &header) {
	FileInBinary in(fileName);
	if (!in.IsOpen())
		throw Exc(Format(t_("Impossible to open file '%s'"), fileName));

	StringBuffer headerB(80);
	in.ReadB(headerB, 80);
	header = headerB;

	if (header.StartsWith("solid"))
		throw Exc(t_("Binary stl must not begin with 'solid' text"));

	/*int32 numFacets =*/ in.ReadB<int32>();

	while (!in.IsEof()) {
    	Vector3D normal;
    	normal.x = double(in.ReadB<float>());
    	normal.y = double(in.ReadB<float>());
    	normal.z = double(in.ReadB<float>());

		Panel &panel = surf.panels.Add();

		Point3D &node0 = surf.nodes.Add();
		node0.x = double(in.ReadB<float>());
		node0.y = double(in.ReadB<float>());
		node0.z = double(in.ReadB<float>());
		panel.id[0] = surf.nodes.GetCount()-1;

		Point3D &node1 = surf.nodes.Add();
		node1.x = double(in.ReadB<float>());
		node1.y = double(in.ReadB<float>());
		node1.z = double(in.ReadB<float>());
		panel.id[1] = surf.nodes.GetCount()-1;

		Point3D &node2 = surf.nodes.Add();
		node2.x = double(in.ReadB<float>());
		node2.y = double(in.ReadB<float>());
		node2.z = double(in.ReadB<float>());
		panel.id[2] = surf.nodes.GetCount()-1;
		panel.id[3] = panel.id[0];

		/*int16 attributeByteCount =*/ in.ReadB<int16>();
	}
}

static void STLFacetBinNodeOut(FileOutBinary &out, const Point3D &node) {
	out.Write(float(node.x));
	out.Write(float(node.y));
	out.Write(float(node.z));
}

void SaveStlBin(String fileName, const Surface &surf) {
	FileOutBinary out(fileName);
	if (!out.IsOpen())
		throw Exc(Format(t_("Impossible to open '%s'\n"), fileName));

	const Vector<Panel> &panels = surf.panels;
	const Vector<Point3D> &nodes = surf.nodes;

	String header = "U++ STL mesh export";
	header << String(' ', 80 - header.GetCount());
	out.Put64(header, 80);

	out.Write(int32(panels.GetCount()));

	for (int i = 0; i < panels.GetCount(); ++i) {
		const Panel &panel = panels[i];

		out.Write(float(panel.normal0.x));
		out.Write(float(panel.normal0.y));
		out.Write(float(panel.normal0.z));
		STLFacetBinNodeOut(out, nodes[panel.id[0]]);
		STLFacetBinNodeOut(out, nodes[panel.id[1]]);
		STLFacetBinNodeOut(out, nodes[panel.id[2]]);
		out.Write(int16(0));

		if (!panel.IsTriangle()) {
			out.Write(float(panel.normal1.x));
			out.Write(float(panel.normal1.y));
			out.Write(float(panel.normal1.z));
			STLFacetBinNodeOut(out, nodes[panel.id[2]]);
			STLFacetBinNodeOut(out, nodes[panel.id[3]]);
			STLFacetBinNodeOut(out, nodes[panel.id[0]]);
			out.Write(int16(0));
		}
	}
}

void LoadStl(String file, Surface &surf, bool &isText, String &header) {
	try {
		LoadStlTxt(file, surf, isText);
	} catch(Exc e) {
		if (!isText)
			LoadStlBin(file, surf, header);
		else
			throw std::move(e);
	}
}

}


/***
 *  $Id$
 **
 *  File: graph_dijkstra.hpp
 *  Created: May 9, 2012
 *
 *  Author: Olga Wodo, Baskar Ganapathysubramanian
 *  Copyright (c) 2012 Olga Wodo, Baskar Ganapathysubramanian
 *  See accompanying LICENSE.
 *
 *  This file is part of GraSPI.
 */

#ifndef GRAPH_DIJKSTRA_HPP
#define GRAPH_DIJKSTRA_HPP

#include "graspi_types.hpp"
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace graspi {
    /// This function determines the shortest paths in the fitered graph.
    ///
    /// This function determines the shortest paths in the filtered graph.
    /// The shortest paths and the lengths (distances) are computed from the source vertex to all vertices in the filtered graph.
    /// The filtered graph is determined using predicate Pred.
    /// @tparam Pred is the predicate used to filter the graph
    /// @param G is the input graph
    /// @param W is the map storing the weights of the edges
    /// @param source is the source vertex with respect to which distances are to be determine_shortest_distanced
    /// @param pred is the predicate used to filter the graph
    /// @param d is the vector of distance to be determined as a result of this function
    template<typename Pred>
    inline void determine_shortest_distances(graph_t*G,
                                             const edge_weights_t& W,
                                             vertex_t source,
                                             const Pred& pred,
                                             std::vector<float>& d){
        boost::filtered_graph<graph_t, Pred> FG(*G,pred);
        unsigned int n = boost::num_vertices(*G);
        std::vector<vertex_t> p(n);
        std::fill(d.begin(), d.end(), 0.0);
        for (unsigned int i = 0; i < n; ++i) p[i] = i;

        boost::dijkstra_shortest_paths(FG, source,
                                       boost::predecessor_map(&p[0])
                                       .
                                       distance_map(&d[0]).weight_map(W));
    }
}//graspi-namespace

#endif


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

#define BOOST_TEST_MODULE FMP_GMapping_Test
#define BOOST_TEST_DYN_LINK
#define BOOST_AUTO_TEST_MAIN
#include <boost/test/unit_test.hpp>

#include "gmapping/scanmatcher/gridlinetraversal.h"
#include "gmapping/utils/autoptr.h"

using namespace GMapping;

typedef autoptr<double> DoubleAutoPtr;

BOOST_AUTO_TEST_CASE(AutoPointerTest) {
  double *d1 = new double(10.);
  double *d2 = new double(20.);

  // Construction test
  DoubleAutoPtr pd1(d1);
  DoubleAutoPtr pd2(d2);
  BOOST_CHECK(*d1 == *pd1);
  BOOST_CHECK(*d2 == *pd2);

  // Copy Construction
  DoubleAutoPtr pd3(pd1);
  BOOST_CHECK(*pd3 == *pd1);

  // Assignment
  pd3 = pd2;
  pd1 = pd2;
  BOOST_CHECK(*pd1 == *pd2 && *pd2 == *pd3);
}

int bresenham_num_points(int x0, int y0, int x1, int y1, bool supercover) {
  IntPoint *points = new IntPoint[20000];
  GridLineTraversalLine line;
  line.points = points;

  IntPoint start = IntPoint(x0, y0);
  IntPoint end = IntPoint(x1, y1);

  if (supercover)
    GridLineTraversal::gridLine(start, end, &line);
  else
    GridLineTraversal::gridLineOld(start, end, &line);

  return line.num_points;
}

BOOST_AUTO_TEST_CASE(LineBresenhamTest) {
  // Check Bresenham algorithm for line discretization.
  bool supercover = false;

  BOOST_CHECK(bresenham_num_points(0, 0, 0, 0, supercover) == 1);       // Line contained in single cell
  BOOST_CHECK(bresenham_num_points(0, 0, 10, 10, supercover) == 11);    // First Quadrant 45º Line
  BOOST_CHECK(bresenham_num_points(-8, 12, -2, 5, supercover) == 8);    // Second Quadrant Line
  BOOST_CHECK(bresenham_num_points(-15, -3, -6, 0, supercover) == 10);  // Third Quadrant Line
  BOOST_CHECK(bresenham_num_points(-3, -4, 0, 1, supercover) == 6);     // Fourth Quadrant Line
  BOOST_CHECK(bresenham_num_points(-2, 3, 9, 11, supercover) ==
              12);  // Tricky for Bresenham (misses about 40% of the cells crossed by the line!)
  BOOST_CHECK(bresenham_num_points(4, 9, 14, 14, supercover) == 11);                 // Slope of 1/2
  BOOST_CHECK(bresenham_num_points(80, -25, 133, 41, supercover) == 67);             // Slope of 2
  BOOST_CHECK(bresenham_num_points(6, 3, 14, 14, supercover) == 12);                 // Same as Above +90º
  BOOST_CHECK(bresenham_num_points(-100, 1, 200, 1, supercover) == 301);             // Long Line
  BOOST_CHECK(bresenham_num_points(-2024, 3029, 9085, 11120, supercover) == 11110);  // Even longer Line
}

BOOST_AUTO_TEST_CASE(LineSuperCoverTest) {
  // Check Line SuperCover algorithm for line discretization.
  bool supercover = true;

  BOOST_CHECK(bresenham_num_points(0, 0, 0, 0, supercover) == 1);       // Line contained in single cell
  BOOST_CHECK(bresenham_num_points(0, 0, 10, 10, supercover) == 11);    // First Quadrant 45º Line
  BOOST_CHECK(bresenham_num_points(-8, 12, -2, 5, supercover) == 14);   // Second Quadrant Line
  BOOST_CHECK(bresenham_num_points(-15, -3, -6, 0, supercover) == 10);  // Third Quadrant Line
  BOOST_CHECK(bresenham_num_points(-3, -4, 0, 1, supercover) == 8);     // Fourth Quadrant Line
  BOOST_CHECK(bresenham_num_points(-2, 3, 9, 11, supercover) ==
              20);  // Tricky for Bresenham (misses about 40% of the cells crossed by the line!)
  BOOST_CHECK(bresenham_num_points(4, 9, 14, 14, supercover) == 16);                 // Slope of 1/2
  BOOST_CHECK(bresenham_num_points(80, -25, 133, 41, supercover) == 120);            // Slope of 2
  BOOST_CHECK(bresenham_num_points(6, 3, 14, 14, supercover) == 20);                 // Same as Above +90º
  BOOST_CHECK(bresenham_num_points(-100, 1, 200, 1, supercover) == 301);             // Long Line
  BOOST_CHECK(bresenham_num_points(-2024, 3029, 9085, 11120, supercover) == 19198);  // Even longer Line
}


#define BOOST_TEST_MODULE "test_point"

#ifdef UNITTEST_FRAMEWORK_LIBRARY_EXIST
#include <boost/test/unit_test.hpp>
#else
#define BOOST_TEST_NO_LIB
#include <boost/test/included/unit_test.hpp>
#endif

#include <periortree/point.hpp>

template<typename T>
perior::point<T, 3> make_point(T v1, T v2, T v3)
{
    perior::point<T, 3> retval;
    retval[0] = v1;
    retval[1] = v2;
    retval[2] = v3;
    return retval;
}

BOOST_AUTO_TEST_CASE(test_point_traits)
{
    BOOST_CHECK((perior::traits::is_point<perior::point<double, 2> >::value));
    BOOST_CHECK((perior::traits::is_point<perior::point<int,    2> >::value));
    BOOST_CHECK((perior::traits::is_point<perior::point<double, 3> >::value));
    BOOST_CHECK((perior::traits::is_point<perior::point<int,    3> >::value));

    BOOST_CHECK((perior::traits::dimension<perior::point<int, 1> >::value) == 1);
    BOOST_CHECK((perior::traits::dimension<perior::point<int, 2> >::value) == 2);
    BOOST_CHECK((perior::traits::dimension<perior::point<int, 3> >::value) == 3);

    BOOST_CHECK((boost::is_same<int,
        perior::traits::scalar_type_of<perior::point<int, 1> >::type>::value));
    BOOST_CHECK((boost::is_same<float,
        perior::traits::scalar_type_of<perior::point<float, 1> >::type>::value));
    BOOST_CHECK((boost::is_same<double,
        perior::traits::scalar_type_of<perior::point<double, 1> >::type>::value));

    perior::point<int, 3> zi =
        perior::traits::zero_vector<perior::point<int, 3> >();
    BOOST_CHECK_EQUAL(zi[0], 0);
    BOOST_CHECK_EQUAL(zi[1], 0);
    BOOST_CHECK_EQUAL(zi[2], 0);

    perior::point<double, 3> zd =
        perior::traits::zero_vector<perior::point<double, 3> >();
    BOOST_CHECK_EQUAL(zd[0], 0);
    BOOST_CHECK_EQUAL(zd[1], 0);
    BOOST_CHECK_EQUAL(zd[2], 0);
}

BOOST_AUTO_TEST_CASE(test_xyz_access)
{
    perior::point<double, 3> p = make_point(1., 2., 3.);
    BOOST_CHECK_EQUAL(p[0], 1.);
    BOOST_CHECK_EQUAL(p[1], 2.);
    BOOST_CHECK_EQUAL(p[2], 3.);

    p[0] = 0.0;
    p[1] = 2.0;
    p[2] = 4.0;

    BOOST_CHECK_EQUAL(p[0], 0.0);
    BOOST_CHECK_EQUAL(p[1], 2.0);
    BOOST_CHECK_EQUAL(p[2], 4.0);
}

BOOST_AUTO_TEST_CASE(test_point_add)
{
    perior::point<double, 3> p = make_point(1., 2., 3.);
    perior::point<double, 3> q = make_point(2., 5., 8.);
    perior::point<double, 3> r = p + q;
    BOOST_CHECK_CLOSE_FRACTION(r[0],  3.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[1],  7.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[2], 11.0, 1e-12);
}

BOOST_AUTO_TEST_CASE(test_point_sub)
{
    perior::point<double, 3> p = make_point(2., 5., 8.);
    perior::point<double, 3> q = make_point(1., 2., 3.);
    perior::point<double, 3> r = p - q;
    BOOST_CHECK_CLOSE_FRACTION(r[0], 1.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[1], 3.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[2], 5.0, 1e-12);
}

BOOST_AUTO_TEST_CASE(test_point_scl_mul)
{
    perior::point<double, 3> p = make_point(1., 2., 3.);
    double q = 3;
    perior::point<double, 3> r = p * q;
    BOOST_CHECK_CLOSE_FRACTION(r[0], 3.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[1], 6.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[2], 9.0, 1e-12);
}

BOOST_AUTO_TEST_CASE(test_position_scl_div)
{
    perior::point<double, 3> p = make_point(1., 2., 3.);
    double q = 3;
    perior::point<double, 3> r = p / q;
    BOOST_CHECK_CLOSE_FRACTION(r[0], 1./3.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[1], 2./3.0, 1e-12);
    BOOST_CHECK_CLOSE_FRACTION(r[2], 3./3.0, 1e-12);
}


#ifndef DEFINE_TYPES_HPP
#define DEFINE_TYPES_HPP

#include <functional>

#include <Eigen/Dense>

using Matrix = Eigen::MatrixXd;
using MatrixDimSizeType = int;

using Vector = Eigen::VectorXd;
using RowVector = Eigen::RowVectorXd;

using BoundaryFunction = std::function<double(double)>;
using FictiveBoundaryFunction = std::function<double(const Vector& v, double x_max, double hx)>;

#endif