//#include <boost/program_options.hpp>
//namespace po = boost::program_options;

//#include <vector>
#include <iostream>
#include <complex>

//#include <stdio.h>

#include <fftw3.h>

//#include <tiffio.h>

#define cimg_use_tiff
#include <CImg.h>
using namespace cimg_library;

#ifdef _WIN32
#define _USE_MATH_DEFINES
#define rint(fp) (int)((fp) >= 0 ? (fp) + 0.5 : (fp) - 0.5)
#endif

#include <math.h>

extern int save_tiff(TIFF *tif, const unsigned int directory, int colind, const int nwaves, int width, int height, float * buffer);
extern int load_tiff(TIFF *const tif, const unsigned int directory, const unsigned colind, float *const buffer);

float fitparabola(float a1, float a2, float a3);
void determine_center_and_background(float *stack5phases, int nx, int ny, int nz, float *xc, float *yc, float *zc, float *background);
void shift_center(std::complex<float> *bands, int nx, int ny, int nz, float xc, float yc, float zc);
void cleanup(std::complex<float> *otfkxkz, int nx, int nz, float dkr, float dkz, float linespacing, int lambdanm, int twolens, float NA, float NIMM);
void radialft(std::complex<float> *bands, int nx, int ny, int nz, std::complex<float> *avg);

bool fixorigin(std::complex<float> *otfkxkz, int nx, int nz, int kx2);
void rescale(std::complex<float> *otfkxkz, int nx, int nz);

#ifdef _WIN32
#ifdef RADIALFT_IMPORT
  #define RADIALFT_API __declspec( dllimport )
#else
  #define RADIALFT_API __declspec( dllexport )
#endif
#else
  #define RADIALFT_API
#endif

extern "C" {
RADIALFT_API int makeOTF(const char *const ifiles, const char *const ofiles,
      int lambdanm = 520, float dz = 0.102, int interpkr = 10,
      bool bUserBackground = false, float background = 90,
      float NA = 1.25, float NIMM = 1.3, float dr = 0.102,
      int krmax = 0, bool bDoCleanup = false);
}

//   ("na", po::value<float>(&NA)->default_value(1.25), "NA of detection objective")
//   ("nimm", po::value<float>(&NIMM)->default_value(1.3), "refractive index of immersion medium")
//   ("xyres", po::value<float>(&dr)->default_value(.102), "x-y pixel size")
//   ("zres", po::value<float>(&dz)->default_value(.102), "z pixel size")
//   ("wavelength", po::value<int>(&lambdanm)->default_value(520), "emission wavelength in nm")
//   ("fixorigin", po::value<int>(&interpkr)->default_value(10),
//    "for all kz, extrapolate using pixels kr=1 to this pixel to get value for kr=0")
//   ("krmax", po::value<int>(&krmax)->default_value(0),
//    "pixels outside this limit will be zeroed (overwriting estimated value from NA and NIMM)")
//   ("nocleanup", po::bool_switch(&bDoCleanup)->implicit_value(false), "elect not to do clean-up outside OTF support")
//   ("background", po::value<float>(&background), "use user-supplied background instead of the estimated")
//   ("input-file", po::value<std::string>(&ifiles)->required(), "input file")
//   ("output-file", po::value<std::string>(&ofiles)->required(), "output file")

std::string ifiles, ofiles;
int nx, ny, nz, nxy;
int i, j, z;
float dkr, dkz, background, estBackground, xcofm, ycofm, zcofm;
float *floatimage;
std::complex<float> *bands, *avg_output;
fftwf_plan rfftplan3d;

// float dr = 0.102;
// float dz = 0.102;
// int interpkr = 10;
// float NA = 1.25;
// float NIMM = 1.3;
// int lambdanm = 520;
// int krmax = 0;
// bool bDoCleanup = true;
// bool bUserBackground = false;

int makeOTF(const char *const ifiles, const char *const ofiles, int lambdanm,
    float dz, int interpkr, bool bUserBackground, float background,
    float NA, float NIMM, float dr, int krmax, bool bDoCleanup)
{
  printf("called");

  TIFFSetWarningHandler(NULL);

  CImg<> rawtiff(ifiles);

  nz = rawtiff.depth();
  ny = rawtiff.height();
  nx = rawtiff.width();

  printf("nx=%d, ny=%d, nz=%d\n", nx, ny, nz);

  dkr = 1/(ny*dr);
  dkz = 1/(nz*dz);

  nxy=(nx+2)*ny;

  floatimage = (float *) malloc(nxy*nz*sizeof(float));
  bands = (std::complex<float> *) floatimage;

 // printf("Reading data...\n\n");

  for(z=0; z<nz; z++)
    for (i=0; i<ny; i++) {
      for (j=0; j<nx; j++)
        floatimage[z*nxy+i*(nx+2)+j] = rawtiff(j, i, z);
    }

  /* Before FFT, estimate bead center position */
  determine_center_and_background(floatimage, nx, ny, nz, &xcofm, &ycofm, &zcofm, &estBackground);

  printf("Center of mass is (%.3f, %.3f, %.3f)\n", xcofm, ycofm, zcofm);

  if (!bUserBackground)
    background = estBackground;

  printf("Background is %.3f\n", background);

  for(z=0; z<nz; z++) {
    for(i=0;i<ny;i++)
      for(j=0;j<nx;j++)
        floatimage[z*nxy + i*(nx+2) + j] -= background;
  }

  rfftplan3d = fftwf_plan_dft_r2c_3d(nz, ny, nx, floatimage,
                                     (fftwf_complex *) floatimage, FFTW_ESTIMATE);

  //printf("Before fft\n");

  fftwf_execute_dft_r2c(rfftplan3d, floatimage, (fftwf_complex *) floatimage);

  fftwf_destroy_plan(rfftplan3d);

  //printf("After fft\n\n");

  /* modify the phase of bands, so that it corresponds to FFT of a bead at origin */
  //printf("Shifting center...\n");

  shift_center(bands, nx, ny, nz, xcofm, ycofm, zcofm);

  CImg<> output_tiff(nz*2, nx/2+1, 1, 1, 0.f);
  avg_output = (std::complex<float> *) output_tiff.data();

  radialft(bands, nx, ny, nz, avg_output);

  if (bDoCleanup)
    cleanup(avg_output, nx, nz, dkr, dkz, 1.0, lambdanm, krmax, NA, NIMM);

  if (interpkr > 0)
    try {
    while (!fixorigin(avg_output, nx, nz, interpkr)) {
      interpkr --;
      if (interpkr < 4)
        throw std::runtime_error("#pixels < 4 used in kr=0 extrapolation");
    }}
  catch (std::exception &e) {
    std::cout << "\n!!Error occurred: " << e.what() << std::endl;
    return 1;
  }

//  printf("%d\n", interpkr);
  rescale(avg_output, nx, nz);

  /* For side bands, combine bandre's and bandim's into bandplus */
  /* Shouldn't this be done later on the averaged bands? */

  output_tiff.save_tiff(ofiles);

  return 0;
}

/*  locate peak pixel to subpixel accuracy by fitting parabolas  */
void determine_center_and_background(float *stack3d, int nx, int ny, int nz, float *xc, float *yc, float *zc, float *background)
{
  int i, j, k, maxi, maxj, maxk, ind, nxy2, infocus_sec;
  int iminus, iplus, jminus, jplus, kminus, kplus;
  float maxval, reval, valminus, valplus;
  double sum;

  //printf("In determine_center_and_background()\n");
  nxy2 = (nx+2)*ny;

  /* Search for the peak pixel */
  /* Be aware that stack3d is of dimension (nx+2)xnyxnz */
  maxval=0.0;
  for(k=0;k<nz;k++)
    for(i=0;i<ny;i++)
      for(j=0;j<nx;j++) {
    ind=k*nxy2+i*(nx+2)+j;
    reval=stack3d[ind];
    if( reval > maxval ) {
      maxval = reval;
      maxi=i; maxj=j;
      maxk=k;
    }
      }

  iminus = maxi-1; iplus = maxi+1;
  if( iminus<0 ) iminus+=ny;
  if( iplus>=ny ) iplus-=ny;
  jminus = maxj-1; jplus = maxj+1;
  if( jminus<0 ) jminus+=nx;
  if( jplus>=nx ) jplus-=nx;
  kminus = maxk-1; kplus = maxk+1;
  if( kminus<0 ) kminus+=nz;
  if( kplus>=nz ) kplus-=nz;

  valminus = stack3d[kminus*nxy2+maxi*(nx+2)+maxj];
  valplus  = stack3d[kplus *nxy2+maxi*(nx+2)+maxj];
  *zc = maxk + fitparabola(valminus, maxval, valplus);

  *zc += 0.6;

  valminus = stack3d[maxk*nxy2+iminus*(nx+2)+maxj];
  valplus  = stack3d[maxk*nxy2+iplus *(nx+2)+maxj];
  *yc = maxi + fitparabola(valminus, maxval, valplus);

  valminus = stack3d[maxk*nxy2+maxi*(nx+2)+jminus];
  valplus  = stack3d[maxk*nxy2+maxi*(nx+2)+jplus];
  *xc = maxj + fitparabola(valminus, maxval, valplus);

  sum = 0;
  infocus_sec = floor(*zc);
  for (i=0; i<*yc-20; i++)
    for (j=0; j<nx; j++)
    sum += stack3d[infocus_sec*nxy2 + i*(nx+2) + j];
  *background = sum / ((*yc-20)*nx);
}

/***************************** fitparabola **********************************/
/*     Fits a parabola to the three points (-1,a1), (0,a2), and (1,a3).     */
/*     Returns the x-value of the max (or min) of the parabola.             */
/****************************************************************************/

float fitparabola( float a1, float a2, float a3 )
{
 float slope,curve,peak;

 slope = 0.5* (a3-a1);         /* the slope at (x=0). */
 curve = (a3+a1) - 2*a2;       /* (a3-a2)-(a2-a1). The change in slope per unit of x. */
 if( curve == 0 )
 {
   printf("no peak: a1=%f, a2=%f, a3=%f, slope=%f, curvature=%f\n",a1,a2,a3,slope,curve);
   return( 0.0 );
 }
 peak = -slope/curve;          /* the x value where slope = 0  */
 if( peak>1.5 || peak<-1.5 )
 {
   printf("bad peak position: a1=%f, a2=%f, a3=%f, slope=%f, curvature=%f, peak=%f\n",a1,a2,a3,slope,curve,peak);
   return( 0.0 );
 }
 return( peak );
}

/* To get rid of checkerboard effect in the OTF bands */
/* (xc, yc, zc) is the estimated center of the point source, which in most cases is the bead */
/* Converted from Fortran code. kz is treated differently than kx and ky. don't know why */
void shift_center(std::complex<float> *bands, int nx, int ny, int nz, float xc, float yc, float zc)
{
  int kin, iin, jin, indin, nxy, kz, kx, ky, kycent, kxcent, kzcent;
  std::complex<float> exp_iphi;
  float phi1, phi2, phi, dphiz, dphiy, dphix;

  kycent = ny/2;
  kxcent = nx/2;
  kzcent = nz/2;
  nxy = (nx/2+1)*ny;

  dphiz = 2*M_PI*zc/nz;
  dphiy = 2*M_PI*yc/ny;
  dphix = 2*M_PI*xc/nx;

  for (kin=0; kin<nz; kin++) {    /* the origin of Fourier space is at (0,0) */
    kz = kin;
    if (kz>kzcent) kz -= nz;
    phi1 = dphiz*kz;      /* first part of phi */
    for (iin=0; iin<ny; iin++) {
      ky = iin;
      if (iin>kycent) ky -= ny;
      phi2 = dphiy*ky;   /* second part of phi */
      for (jin=0; jin<kxcent+1; jin++) {
        kx = jin;
        indin = kin*nxy+iin*(nx/2+1)+jin;
        phi = phi1+phi2+dphix*kx;  /* third part of phi */
        /* kz part of Phi has a minus sign, I don't know why. */
        exp_iphi = std::complex<float> (cos(phi), sin(phi));
        bands[indin] = bands[indin] * exp_iphi;
      }
    }
  }
}

void radialft(std::complex<float> *band, int nx, int ny, int nz, std::complex<float> *avg_output)
{
  int kin, iin, jin, indin, indout, indout_conj, kz, kx, ky, kycent, kxcent, kzcent;
  int *count, nxz, nxy;
  float rdist;

  printf("In radialft()\n");
  kycent = ny/2;
  kxcent = nx/2;
  kzcent = nz/2;
  nxy = (nx/2+1)*ny;
  nxz = (nx/2+1)*nz;

  count = (int *) calloc(nxz, sizeof(int));

  if (!count) {
    printf("No memory availale in radialft()\n");
    exit(-1);
  }

  for (kin=0; kin<nz; kin++) {
    kz = kin;
    if (kin>kzcent) kz -= nz;
    for (iin=0; iin<ny; iin++) {
      ky = iin;
      if (iin>kycent) ky -= ny;
      for (jin=0; jin<kxcent+1; jin++) {
        kx = jin;
        rdist = sqrt(kx*kx+ky*ky);
        if (rdist < nx/2+1) {
          indin = kin*nxy+iin*(nx/2+1)+jin;
          indout = rint(rdist)*nz+kin;
          if (indout < nxz) {
            avg_output[indout] += band[indin];
            count[indout] ++;
          }
          // printf("kz=%d, ky=%d, kx=%d, indout=%d\n", kz, ky, kx, indout);
        }
      }
    }
  }

  for (indout=0; indout<nxz; indout++) {
    if (count[indout]>0) {
      avg_output[indout] /= count[indout];
    }
  }

  /* Then complete the rotational averaging and scaling*/
  for (kx=0; kx<nx/2+1; kx++) {
    indout = kx*nz+0;
    avg_output[indout] = std::complex<float>(avg_output[indout].real(), 0);
    for (kz=1; kz<=nz/2; kz++) {
      indout = kx*nz+kz;
      indout_conj = kx*nz + (nz-kz);
      avg_output[indout] = (avg_output[indout] + conj(avg_output[indout_conj])) / 2.f;
      avg_output[indout_conj] = conj(avg_output[indout]);
    }
  }
  free(count);
}

void cleanup(std::complex<float> *otfkxkz, int nx, int nz, float dkr, float dkz, float linespacing, int lamdanm, int krmax_user, float NA, float NIMM)
{
  int ix, iz, kzstart, kzend, icleanup=nx/2+1;
  float lamda, sinalpha, cosalpha, kr, krmax, beta, kzedge;

  lamda = lamdanm * 0.001;
  sinalpha = NA/NIMM;
  cosalpha = cos(asin(sinalpha));
  krmax = 2*NA/lamda;
  if (krmax_user*dkr<krmax && krmax_user!=0)
    krmax = krmax_user*dkr;

  printf("krmax=%f, lambda=%f\n", krmax, lamda);
  for (ix=0; ix<icleanup; ix++) {
    kr = ix * dkr;
    if ( kr <= krmax ) {
      beta = asin( ( NA - kr*lamda ) /NIMM );
      kzedge = (NIMM/lamda) * ( cos(beta) - cosalpha );
      /* kzstart = floor((kzedge/dkz) + 1.999); */ /* In fortran, it's 2.999 */
      kzstart = rint((kzedge/dkz) + 1);
      kzend = nz - kzstart;
      for (iz=kzstart; iz<=kzend; iz++)
        otfkxkz[ix*nz+iz] = 0;
    }
    else {   /* outside of lateral resolution limit */
      for (iz=0; iz<nz; iz++)
        otfkxkz[ix*nz+iz] = 0;
    }
  }
}

bool fixorigin(std::complex<float> *otfkxkz, int nx, int nz, int kx2)
{
  // linear fit the value at kx=0 using kx in [1, kx2]
  double mean_kx = (kx2+1)/2.; // the mean of [1, 2, ..., n] is (n+1)/2

  // printf("In fixorigin(), kx2=%d\n", nx, nz, kx2);

  for (int z=0; z<nz; z++) {
    std::complex<double> mean_val=0;
    std::complex<double> slope_numerator=0;
    std::complex<double> slope_denominat=0;

    for (int x=1; x<=kx2; x++)
      mean_val += otfkxkz[x*nz + z];

    mean_val /= kx2;
    for (int x=1; x<=kx2; x++) {
      std::complex<double> complexval = otfkxkz [x*nz+z];
      slope_numerator += (x - mean_kx) * (complexval - mean_val);
      slope_denominat += (x - mean_kx) * (x - mean_kx);
    }
    std::complex<double> slope = slope_numerator / slope_denominat;
    otfkxkz[z] = mean_val - slope * mean_kx;  // intercept at kx=0
    if (z==0 && std::abs(otfkxkz[z]) <= std::abs(otfkxkz[nz+z])) {
      return false; // indicating kx2 may be too large
    }
  }
  return true;
}

void rescale(std::complex<float> *otfkxkz, int nx, int nz)
{
  int nxz, ind;
  float valmax=0, mag, scalefactor;

  nxz = (nx/2+1)*nz;
  for (ind=0; ind<nxz; ind++) {
    mag = abs(otfkxkz[ind]);
    if (mag > valmax)
      valmax = mag;
  }
  scalefactor = 1/valmax;

  for (ind=0; ind<nxz; ind++) {
    otfkxkz[ind] *= scalefactor;
  }
}


/*
 * main.cpp
 *
 *  Created on: 22/mag/2012
 *      Author: Fabrizio
 */

#include <boost/iterator/counting_iterator.hpp>

#include <iostream>
#include <algorithm>
#include <vector>

int main(int argc, char *argv[]) {

	// This is a naive example of counting from 1 to 100.
	{
		std::cout << "Counting from 1 to 100." << std::endl;
		boost::counting_iterator<int> begin(1), end(100);
		std::copy(begin, end, std::ostream_iterator<int>(std::cout, " "));
		std::cout << std::endl;
	}

	// It may be useful if you want to initialize a vector.
	{
		boost::counting_iterator<int> begin(0), end(10);
		std::vector<int> v;
		std::copy(begin, end, std::back_inserter(v));
	}

	return EXIT_SUCCESS;
}


#define BOOST_TEST_MODULE MoRTO_Test

#include <boost/test/unit_test.hpp>
#include "morto.h"
#include <vector>

using namespace std;
using namespace morto;

BOOST_AUTO_TEST_SUITE(incorrect_input);

BOOST_AUTO_TEST_CASE(DieOnEmpty)
{
	vector<size_t> parents = {  };
	vector<double> values = {  };

	BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values), MortoException);
}

BOOST_AUTO_TEST_CASE(DieOnOutOfDomain)
{
    vector<size_t> parents = {0 , 3};
    vector<double> values = {0, 0};

    BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values), MortoException);
}

BOOST_AUTO_TEST_CASE(DieOnWrongSizeValues)
{
    vector<size_t> parents = {0 , 1};
    vector<double> values = {0};

    BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values), MortoException);
}

BOOST_AUTO_TEST_CASE(DieOnNotTopologicalOrder)
{
    vector<size_t> parents = {1 , 0};
    vector<double> values = {0, 0};

    BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values), MortoException);
}

BOOST_AUTO_TEST_CASE(DieOnWrongSizeWeights)
{
    vector<size_t> parents = {0 , 1};
    vector<double> values = {0, 0};
    vector<double> weights = {0};
    BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values, weights), MortoException);
}

BOOST_AUTO_TEST_CASE(DieOnNegativeWeight)
{
    vector<size_t> parents = {0 , 1};
    vector<double> values = {0, 0};
    vector<double> weights = {1, -2};
    BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values, weights), MortoException);
}

BOOST_AUTO_TEST_CASE(DieOnZeroWeight)
{
    vector<size_t> parents = {0 , 1};
    vector<double> values = {0, 0};
    vector<double> weights = {1, 0};
    BOOST_REQUIRE_THROW(monotonicRegressionOnTree(parents, values, weights), MortoException);
}

BOOST_AUTO_TEST_SUITE_END();

bool testQuasiEquality(vector<double> v1, vector<double> v2)
{
    if (v1.size()!=v2.size())
        return false;
    for(int i=0; i<v1.size(); ++i)
        if(abs(v1[i]-v2[i])>1E-8)
            return false;
    return true;
}

BOOST_AUTO_TEST_SUITE(valid_input);

BOOST_AUTO_TEST_CASE(IdempotentOnIncreasingValues)
{
    vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
    vector<double> values = { 0,1,2,3,4,5,6,7 };
    vector<double> result = monotonicRegressionOnTree(parents, values);

    BOOST_CHECK(testQuasiEquality(values,result));
}

BOOST_AUTO_TEST_CASE(IdempotentOnIncreasingValuesWithWeights)
{
    vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
    vector<double> values = { 0,1,2,3,4,5,6,7 };
    vector<double> weights = { 1,2,3,4,5,6,7,8 };
    vector<double> result = monotonicRegressionOnTree(parents, values, weights);

    BOOST_CHECK(testQuasiEquality(values,result));
}

BOOST_AUTO_TEST_CASE(IdempotentOnIncreasingValuesWithWeightsOnForest)
{
    vector<size_t> parents = { 4,4,6,6,4,5,6 };
    vector<double> values = { 0,1,2,3,4,5,6 };
    vector<double> weights = { 1,2,3,4,5,6,7 };
    vector<double> result = monotonicRegressionOnTree(parents, values, weights);

    BOOST_CHECK(testQuasiEquality(values,result));
}

BOOST_AUTO_TEST_CASE(NormalNoWeights)
{
    vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
    vector<double> values = { 13,14,6,8,11,7,5,10 };

    vector<double> result = monotonicRegressionOnTree(parents, values);

    BOOST_CHECK(testQuasiEquality({12, 12, 6, 6.5, 12, 7, 6.5, 12},result));
}

BOOST_AUTO_TEST_CASE(NormalWeights)
{
    vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
    vector<double> values = { 13,14,6,8,11,7,5,10 };
    vector<double> weights = { 1,1,1,1,1,1,2,1 };
    vector<double> result = monotonicRegressionOnTree(parents, values, weights);

    BOOST_CHECK(testQuasiEquality({12, 12, 6, 6, 12, 7, 6, 12},result));
}

BOOST_AUTO_TEST_CASE(NormalWeightsForest)
{
    vector<size_t> parents = { 4,4,6,6,4,5,6 };
    vector<double> values = { 14,14,6,8,11,7,5};
    vector<double> weights = { 1,1,1,1,1,1,2 };
    vector<double> result = monotonicRegressionOnTree(parents, values, weights);

    BOOST_CHECK(testQuasiEquality({13, 13, 6, 6, 13, 7, 6},result));
}

BOOST_AUTO_TEST_SUITE_END();
/*
BOOST_AUTO_TEST_CASE(MoRTO_Test)
{
	vector<size_t> parents = { 4,4,6,6,7,7,7,7 };
	//vector<double> values = { 0,1,2,3,4,5,6,7 };
	vector<double> values = { 13,14,6,8,11,7,5,10 };
	vector<double> result = monotonicRegressionOnTree(parents, values);

	BOOST_CHECK(test_object.is_valid());
}*/


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "StridedSlice.hpp"

#include <boost/assert.hpp>
#include <boost/numeric/conversion/cast.hpp>

namespace armnn
{

void PadParams(StridedSliceDescriptor& p, unsigned int dimCount)
{
    BOOST_ASSERT_MSG(dimCount <= 4, "Expected input with at most 4 dimensions");

    const unsigned int beginIndicesCount =
        boost::numeric_cast<unsigned int>(p.m_Begin.size());

    BOOST_ASSERT(dimCount >= beginIndicesCount);
    const unsigned int padCount = dimCount - beginIndicesCount;

    p.m_Begin.resize(dimCount);
    p.m_End.resize(dimCount);
    p.m_Stride.resize(dimCount);

    for (unsigned int i = beginIndicesCount; i > 0; --i)
    {
        p.m_Stride[i + padCount - 1] = p.m_Stride[i - 1];
        p.m_Begin[i + padCount - 1] = p.m_Begin[i - 1];
        p.m_End[i + padCount - 1] = p.m_End[i - 1];
    }

    for (unsigned int i = 0; i < padCount; ++i)
    {
        p.m_Stride[i] = 1;
        p.m_Begin[i] = 0;
        p.m_End[i] = 0;
    }

    p.m_ShrinkAxisMask <<= padCount;
    p.m_EllipsisMask <<= padCount;
    p.m_NewAxisMask <<= padCount;
    p.m_BeginMask <<= padCount;
    p.m_EndMask <<= padCount;
    p.m_BeginMask |= (1 << padCount) - 1;
    p.m_EndMask |= (1 << padCount) - 1;
}

bool LoopCondition(int index, int stop, int stride)
{
    return stride > 0 ? index >= stop : index <= stop;
}

TensorShape ExtendShape(const TensorShape& inputShape,
                        unsigned int newNumDimensions)
{
    if (inputShape.GetNumDimensions() >= newNumDimensions)
    {
        return inputShape;
    }

    unsigned int newSizes[newNumDimensions];

    unsigned int diff = newNumDimensions - inputShape.GetNumDimensions();

    for (unsigned int i = 0; i < diff; i++)
    {
        newSizes[i] = 1;
    }

    for (unsigned int i = diff; i < newNumDimensions; i++)
    {
        newSizes[i] = inputShape[i - diff];
    }

    return TensorShape(newNumDimensions, newSizes);
}

template<typename T>
void StridedSlice(const TensorInfo& inputInfo,
                  const TensorInfo& outputInfo,
                  const StridedSliceDescriptor& params,
                  const T* inputData,
                  T* outputData)
{
    const TensorShape inputShape =
        ExtendShape(inputInfo.GetShape(), 4);

    StridedSliceDescriptor paddedParams = params;

    // Pad parameters to 4 dimensions
    PadParams(paddedParams, 4);

    const int start0 =
        paddedParams.GetStartForAxis(inputShape, 0);
    const int stop0 =
        paddedParams.GetStopForAxis(inputShape, 0, start0);

    const int start1 =
        paddedParams.GetStartForAxis(inputShape, 1);
    const int stop1 =
        paddedParams.GetStopForAxis(inputShape, 1, start1);

    const int start2 =
        paddedParams.GetStartForAxis(inputShape, 2);
    const int stop2 =
        paddedParams.GetStopForAxis(inputShape, 2, start2);

    const int start3 =
        paddedParams.GetStartForAxis(inputShape, 3);
    const int stop3 =
        paddedParams.GetStopForAxis(inputShape, 3, start3);

    T* outPtr = outputData;

    for (int in0 = start0;
         !LoopCondition(in0, stop0, paddedParams.m_Stride[0]);
         in0 += paddedParams.m_Stride[0])
    {
        for (int in1 = start1;
             !LoopCondition(in1, stop1, paddedParams.m_Stride[1]);
             in1 += paddedParams.m_Stride[1])
        {
            for (int in2 = start2;
                 !LoopCondition(in2, stop2, paddedParams.m_Stride[2]);
                 in2 += paddedParams.m_Stride[2])
            {
                for (int in3 = start3;
                     !LoopCondition(in3, stop3, paddedParams.m_Stride[3]);
                     in3 += paddedParams.m_Stride[3])
                {
                    int dim1 = boost::numeric_cast<int>(inputShape[1]);
                    int dim2 = boost::numeric_cast<int>(inputShape[2]);
                    int dim3 = boost::numeric_cast<int>(inputShape[3]);

                    int inputOffset = ((in0 * dim1 + in1) * dim2 + in2) * dim3 + in3;
                    *(outPtr++) = inputData[inputOffset];
                }
            }
        }
    }
}

template void StridedSlice<float>(const TensorInfo& inputInfo,
                                  const TensorInfo& outputInfo,
                                  const StridedSliceDescriptor& params,
                                  const float* inputData,
                                  float* outData);

template void StridedSlice<uint8_t>(const TensorInfo& inputInfo,
                                    const TensorInfo& outputInfo,
                                    const StridedSliceDescriptor& params,
                                    const uint8_t* inputData,
                                    uint8_t* outData);

} //namespace armnn


/******************************************************************************
 * Copyright 2018 The Apollo Authors. All Rights Reserved.
 *
 * 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.
 *****************************************************************************/

#include "cyber/cyber.h"
#include "modules/drivers/proto/pointcloud.pb.h"
//#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/common/transforms.h>
#include <pcl/common/common.h>
#include <boost/thread/thread.hpp>
#include <vector>
#include <unordered_set>
#include <string>
#include <chrono>
//#include <vtkProp3D.h>

using namespace std;

//pcl::visualization::CloudViewer viewer("point cloud viewer");
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("viewer test"));

//init para

struct Color
{

	float r, g, b;

	Color(float setR, float setG, float setB)
		: r(setR), g(setG), b(setB)
	{}
};

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
};

float filterRes = 0.4;
Eigen::Vector4f minpoint(-10, -6.5, -2, 1);
Eigen::Vector4f maxpoint(30, 6.5, 1, 1);
int maxIterations = 40;
float distanceThreshold = 0.3;
float clusterTolerance = 0.5;
int minsize = 10;
int maxsize = 140;

void MessageCallback(
		const std::shared_ptr<apollo::drivers::PointCloud>& msg) {
  cout << "-----------------------" << msg->point_size() << endl;
//init viewer
//  viewer->initCameraParameters();
//  int v1(0);
//  int v2(0);
//  viewer->createViewPort(0.0, 0.0, 1.0, 1.0, v1);
//  viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);

  pcl::PointCloud<pcl::PointXYZ>::Ptr pcloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>& cloud = *pcloud;
//  pcl::PointCloud<pcl::PointXYZ> cloud;
  cloud.width = msg->point_size();
  cloud.height = 1;
  cloud.is_dense = false;
  cloud.points.resize(cloud.width * cloud.height);
  for (size_t i = 0; i < cloud.points.size(); ++i) {
    cloud.points[i].x = msg->point(i).x();
    cloud.points[i].y = msg->point(i).y();
    cloud.points[i].z = msg->point(i).z();
  }

//  save point_cloud to pcd file
//  pcl::io::savePCDFileASCII("/apollo/write_pcd_test.pcd", cloud);

// visualize point_cloud
//  viewer.showCloud(pcloud);

//voxel frid filter
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  vg.setInputCloud(pcloud);
  vg.setLeafSize(filterRes, filterRes, filterRes);
  vg.filter(*cloudFiltered);
//  viewer.showCloud(pcloud);

//cropbox
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudRegion(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::CropBox<pcl::PointXYZ> region(true);
  region.setMin(minpoint);
  region.setMax(maxpoint);
  region.setInputCloud(cloudFiltered);
  region.filter(*cloudRegion);

  vector<int> indices;
  pcl::CropBox<pcl::PointXYZ> roof(true);
  roof.setMin(Eigen::Vector4f(-1.5, -1.7, -1, 1));
  roof.setMax(Eigen::Vector4f(2.6, 1.7, -0.4, 1));
  roof.setInputCloud(cloudRegion);
  roof.filter(indices);

  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  for(int point:indices) {
    inliers->indices.push_back(point);
  }

  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud(cloudRegion);
  extract.setIndices(inliers);
  extract.setNegative(true);
  extract.filter(*cloudRegion);

//segment plane
//  int num_points = cloudRegion->points.size();
//  auto cloud_points = cloudRegion->points;
//  Ransac<pcl::PointXYZ> RansacSeg(maxIterations, distanceThreshold, num_points);
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_seg(new pcl::PointIndices);
  pcl::SACSegmentation<pcl::PointXYZ>seg;
  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(distanceThreshold);
  seg.setInputCloud(cloudRegion);
  seg.segment(*inliers_seg, *coefficients);
//segment obstacles
  pcl::PointCloud<pcl::PointXYZ>::Ptr obstCloud(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr planeCloud(new pcl::PointCloud<pcl::PointXYZ>());
  for(int index : inliers_seg->indices) {
    planeCloud->points.push_back(cloudRegion->points[index]);
  }
  pcl::ExtractIndices<pcl::PointXYZ> extract_obst;
  extract_obst.setInputCloud(cloudRegion);
  extract_obst.setIndices(inliers_seg);
  extract_obst.setNegative(true);
  extract.filter(*obstCloud);
  std::pair<pcl::PointCloud<pcl::PointXYZ>::Ptr, pcl::PointCloud<pcl::PointXYZ>::Ptr> segResult(obstCloud, planeCloud);

//cluster
  vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>clusters;
  typename pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud(segResult.first);
  vector<pcl::PointIndices> clusterIndices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance(clusterTolerance);
  ec.setMinClusterSize(minsize);
  ec.setMaxClusterSize(maxsize);
  ec.setSearchMethod(tree);
  ec.setInputCloud(segResult.first);
  ec.extract(clusterIndices);

  for(pcl::PointIndices getIndices: clusterIndices){
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudCluster(new pcl::PointCloud<pcl::PointXYZ>);
    for(int index:getIndices.indices) {
      cloudCluster->points.push_back(segResult.first->points[index]);
    }
    cloudCluster->width = cloudCluster->points.size();
    cloudCluster->height = 1;
    cloudCluster->is_dense = true;
    clusters.push_back(cloudCluster);
  }

//bounding box
  int clusterId = 0;
  vector<Color> colors = {Color(1, 0, 0), Color(0, 1, 0), Color(0, 0, 1)};
  for(pcl::PointCloud<pcl::PointXYZ>::Ptr cluster_temp: clusters) {
//    viewer->addPointCloud<pcl::PointXYZ>(cluster_temp, "obstcloud" + std::to_string(clusterId));
    pcl::PointXYZ minpoint_temp, maxpoint_temp;
    pcl::getMinMax3D(*cluster_temp, minpoint_temp, maxpoint_temp);

    Box box;
    box.x_min = minpoint_temp.x;
    box.y_min = minpoint_temp.y;
    box.z_min = minpoint_temp.z;
    box.x_max = maxpoint_temp.x;
    box.y_max = maxpoint_temp.y;
    box.z_max = maxpoint_temp.z;
    cout << box.x_min << endl;
    ++clusterId;
  }
//  viewer->setBackgroundColor (0, 0, 0);
//  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> pcloud_handler(pcloud, 250, 0, 0);
//  viewer->addPointCloud(pcloud, "window1", v1);
//  viewer->addPointCloud(pcloud, "cloud", v1);
//  viewer->addPointCloud(pcloud, pcloud_handler, "window2", v2);
//  viewer->spin();

//  viewer.showCloud(cloudRegion);
}

int main(int argc, char* argv[]) {
  // init cyber framework
  apollo::cyber::Init(argv[0]);
  // create listener node
  auto listener_node = apollo::cyber::CreateNode("listener");
  // create listener
  auto listener = listener_node->CreateReader<apollo::drivers::PointCloud>(
		  "/diamond/sensor/lidar/front/PointCloud2", MessageCallback);

  apollo::cyber::WaitForShutdown();
  return 0;
}


/*
@copyright Louis Dionne 2014
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/hana/orderable/orderable.hpp>

#include <boost/hana/core/datatype.hpp>
#include <boost/hana/core/is_a.hpp>

#include <boost/hana/detail/assert.hpp>
using namespace boost::hana;

struct integer {
    int value;
    friend constexpr bool operator<(integer a, integer b)
    { return a.value < b.value; }
};

struct integer2 {
    int value;
    struct hana_datatype;
    friend constexpr bool operator<(integer2 a, integer2 b)
    { return a.value < b.value; }
};

using Integer = datatype_t<integer>;
using Integer2 = datatype_t<integer2>;

int main() {
    BOOST_HANA_CONSTEXPR_ASSERT(less(integer{0}, integer{1}));
    BOOST_HANA_CONSTEXPR_ASSERT(!less(integer{0}, integer{0}));
    BOOST_HANA_CONSTEXPR_ASSERT(!less(integer{1}, integer{0}));
    BOOST_HANA_CONSTANT_ASSERT(are<Orderable, Integer, Integer>);

    // No instance is provided when the data type differs from the C++ type.
    BOOST_HANA_CONSTANT_ASSERT(!are<Orderable, Integer2, Integer2>);
    BOOST_HANA_CONSTANT_ASSERT(!are<Orderable, Integer2, Integer>);
    BOOST_HANA_CONSTANT_ASSERT(!are<Orderable, Integer, Integer2>);
}


#include <boost/chrono.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/heap/priority_queue.hpp>

#include <math.h>

#include "Forest.h"
#include "GraphDefs.h"
#include "GraphUtil.h"
#include "GraphParse.h"
#include "GraphGen.h"
#include "Mst.h"
#include "Timer.h"

#include <string>

#define SHOW_DEBUG false
#define SHOW_INPUT_SYNTAX false

void printGraph(const char* title, BoostGraph* g, bool printVertices = false) {
	std::cout << std::endl << "=== " << title << std::endl;

	if (printVertices) {
		// Iterate through the vertices and print them out
		std::cout << "Vertices: " << std::endl;
		std::pair<VertexIter, VertexIter> vp;
		for (vp = boost::vertices(*g); vp.first != vp.second; ++vp.first) {
			Vertex v = *vp.first;
			std::cout << "index: " << v << " (" << (*g)[v].pt.x() << ", " << (*g)[v].pt.y() << ")" << std::endl;
		}

		if (SHOW_INPUT_SYNTAX) {
			for (vp = boost::vertices(*g); vp.first != vp.second; ++vp.first) {
				Vertex v = *vp.first;
				std::cout << "Vertex v" << v << " = add_vertex(*F); (*F)[v" << v << "].pt = CGALPoint(" << (*g)[v].pt.x() << ", " << (*g)[v].pt.y() << ");" << std::endl;
			}

			std::pair<EdgeIter, EdgeIter> ep;
			EdgeIter ei, ei_end;
			for (tie(ei, ei_end) = boost::edges(*g); ei != ei_end; ++ei) {
				Edge e = *ei;
				Vertex src = source(e, *g);
				Vertex tar = target(e, *g);
				std::cout << "add_edge(v" << src << ", v" << tar << ", *F);" << std::endl;
			}
		}
	}

	// Iterate through the edges and print them out
	std::cout << std::endl << "Edges: " << std::endl;
	std::pair<EdgeIter, EdgeIter> ep;
	EdgeIter ei, ei_end;
	for (tie(ei, ei_end) = boost::edges(*g); ei != ei_end; ++ei) {
		Edge e = *ei;
		Vertex src = source(e, *g);
		Vertex tar = target(e, *g);
		//std::cout << edgeWeightMap->at(e).weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
		std::cout << (*g)[e].weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
	}

	std::cout << std::endl;
}

void printGraph(const char* title, VertexVector* vertices, EdgeVector* edges, bool printVertices = false) {
	std::cout << std::endl << "=== " << title << std::endl;

	if (printVertices) {
		// Iterate through the vertices and print them out
		for (int i = 0; i < vertices->size(); i++) {
			CGALPoint* v = (*vertices)[i];
			std::cout << "index: " << v << " (" << (*v) << ")" << std::endl;
		}
	}

	// Iterate through the edges and print them out
	std::cout << std::endl << "Edges: " << std::endl;
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		//std::cout << edgeWeightMap->at(e).weight << " (" << (*g)[src].pt << ") (" << (*g)[tar].pt << ")" << std::endl;
		std::cout << e->weight << " (" << (*src) << ") (" << (*tar) << ")" << std::endl;
	}

	std::cout << std::endl;
}

//#include <iostream>
//#include <fstream>

void printGDFGraph(const char* fileName, VertexVector* vertices, EdgeVector* edges) {
	std::ofstream myfile;
	myfile.open(fileName, std::ios::out | std::ios::in);

	myfile << "nodedef> name VARCHAR,label VARCHAR,width DOUBLE,height DOUBLE,x DOUBLE,y DOUBLE,color VARCHAR" << std::endl;
	//std::cout << "nodedef> name VARCHAR,label VARCHAR,width DOUBLE,height DOUBLE,x DOUBLE,y DOUBLE,color VARCHAR" << std::endl;

	// Iterate through the vertices and print them out
	boost::unordered_map<CGALPoint*, VertexIndex> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* v = (*vertices)[i];
		myfile << i << ",,10.0,10.0," << (*v).x() << "," << (*v).y() << ",'153,153,153'" << std::endl;
		//std::cout << i << ",,10.0,10.0," << (*v).x() << "," << (*v).y() << ",'153,153,153'" << std::endl;
		vertexHandles.emplace(v, i);
	}

	myfile << "edgedef> node1,node2,weight DOUBLE,directed BOOLEAN,color VARCHAR" << std::endl;
	//std::cout << "edgedef> node1,node2,weight DOUBLE,directed BOOLEAN,color VARCHAR" << std::endl;

	// Iterate through the edges and print them out
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* e = (*edges)[i];
		CGALPoint* src = (*vertices)[e->u];
		CGALPoint* tar = (*vertices)[e->v];
		VertexIndex srcInd = vertexHandles[src];
		VertexIndex tarInd = vertexHandles[tar];
		//EdgeWeight weight = sqrt(CGAL::squared_distance(*src, *tar));
		EdgeWeight weight = 1.0;
		myfile << srcInd << "," << tarInd << "," << weight << ",false,'128,128,128'" << std::endl;
		//std::cout << srcInd << "," << tarInd << "," << weight << ",false,'128,128,128'" << std::endl;
	}

	myfile.close();
	//std::cout << std::endl;
}

CDT* computeCdt(VertexVector* vertices, EdgeVector* edges) {
	CDT* cdt = new CDT();

	boost::unordered_map<VertexIndex, Vertex_handle> vertexHandles;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* pt = (*vertices)[i];
		Vertex_handle vHandle = cdt->insert(*pt);
		vertexHandles.emplace(i, vHandle);
	}

	// Insert constraint edges
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* edge = (*edges)[i];
		VertexIndex u = edge->u;
		VertexIndex v = edge->v;
		Vertex_handle uH = vertexHandles[u];
		Vertex_handle vH = vertexHandles[v];
		cdt->insert_constraint(uH, vH);
	}

	assert(cdt->is_valid());

	return cdt;
}

void printCdtInfo(CDT* cdt) {
	std::cout << std::endl << "=== Contraint Delaunay Triangulation" << std::endl;
	int constraintCount = 0, unconstraintCount = 0;
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge e = *eit;
		if (cdt->is_constrained(e)) {
			constraintCount++;
			std::cout << "C ";
		}
		else {
			unconstraintCount++;
			std::cout << "U ";
		}

		CGALSegment s = cdt->segment(e);
		const CGALPoint& p1 = s.point(0);
		const CGALPoint& p2 = s.point(1);
		std::cout << "(" << p1 << ") (" << p2 << ")" << std::endl;
	}
	std::cout << "# constrained edges " << constraintCount << std::endl;
	std::cout << "# unconstrained edges " << unconstraintCount << std::endl << std::endl;

	/*for (CDT::Vertex_iterator vit = cdt->vertices_begin(); vit != cdt->vertices_end(); ++vit) {
	CDT::Vertex v = *vit;
	std::cout << "(" << v << ")" << std::endl;
	}*/
}

// Given 3 colinear points, if the 2 furthest points are constrained, how is this delaunay triangulated?
// It seems the outcome is implementation dependent. CGAL will split the constraint into 2 edges.
// Other implementations might allow for overlapping, collinear edges. Either way, the current plan
// is to assume that an input forest, F, with collinear edges will be replaced by smaller constraint edges.
EdgeVector* newConstraintSetFromCdt(CDT* cdt, VertexVector* originalVertices) {
	EdgeVector* newEdgeVector = new EdgeVector();

	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < originalVertices->size(); i++) {
		vertexIndex[*(*originalVertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;

		if (cdt->is_constrained(cgal_e)) {
			// Assumes point coord are unique
			CGALSegment segement = cdt->segment(cgal_e);
			CGALPoint cgal_u = segement.point(0);
			CGALPoint cgal_v = segement.point(1);
			VertexIndex u = vertexIndex[cgal_u];
			VertexIndex v = vertexIndex[cgal_v];

			SimpleEdge* edge = new SimpleEdge(u, v, 0);
			newEdgeVector->push_back(edge);
		}
	}
	return newEdgeVector;
}

EdgeVector* convertCdtToGraph(VertexVector* vertices, CDT* cdt) {
	EdgeVector* edgeVec = new EdgeVector();

	// Map CGALPoint -> VertexIndex
	boost::unordered_map<CGALPoint, VertexIndex> vertexIndex;
	for (int i = 0; i < vertices->size(); i++) {
		vertexIndex[*(*vertices)[i]] = i;
	}

	// Add edges to graph
	for (CDT::Edge_iterator eit = cdt->edges_begin(); eit != cdt->edges_end(); ++eit) {
		CDT::Edge cgal_e = *eit;
		CGALSegment segement = cdt->segment(cgal_e);
		CGALPoint cgal_u = segement.point(0);
		CGALPoint cgal_v = segement.point(1);
		VertexIndex u = vertexIndex[cgal_u];
		VertexIndex v = vertexIndex[cgal_v];

		SimpleEdge* edge = new SimpleEdge(u, v, 0);
		edgeVec->push_back(edge);
	}

	return edgeVec;
}

BoostGraph* convertVectorToGraph(VertexVector* vertices, EdgeVector* edges, boost::unordered_map<Edge, SimpleEdge*>** boostToSimpleEdgeMap, boost::unordered_map<SimpleEdge, SimpleEdge*>* relativeWeightSet) {
	BoostGraph* g = new BoostGraph();

	// Add vertices to graph
	boost::unordered_map<VertexIndex, Vertex> verticesMap;
	for (int i = 0; i < vertices->size(); i++) {
		CGALPoint* pt = (*vertices)[i];
		Vertex gv = add_vertex(*g);
		(*g)[gv].pt = *pt;
		verticesMap.emplace(i, gv);
	}

	// Map BoostEdge -> SimpleEdge
	(*boostToSimpleEdgeMap) = new boost::unordered_map<Edge, SimpleEdge*>();

	// Add edges to graph
	for (int i = 0; i < edges->size(); i++) {
		SimpleEdge* edge = (*edges)[i];
		Vertex u = verticesMap[edge->u];
		Vertex v = verticesMap[edge->v];
		std::pair<Edge, bool> result = add_edge(u, v, *g);
		assert(result.second);

		SimpleEdge* cdtEdge = (*relativeWeightSet)[(*edge)];
		(**boostToSimpleEdgeMap)[result.first] = cdtEdge;
	}

	return g;
}

struct tree_visitor : boost::default_bfs_visitor {
public:
	Forest* forest;
	boost::unordered_map<Edge, SimpleEdge*>* boostToSimpleEdgeMap;

	tree_visitor(Forest* f, boost::unordered_map<Edge, SimpleEdge*>* edgeMap) : forest(f), boostToSimpleEdgeMap(edgeMap) {}

	void tree_edge(const Edge &e, const BoostGraph &g) const {
		Vertex src = source(e, g);
		Vertex tar = target(e, g);
		//CGAL::Lazy_exact_nt<CGAL::Gmpq> exactWeight = CGAL::squared_distance(g[src].pt, g[tar].pt);
		//double weight = CGAL::to_double(exactWeight); // Re-calculate weights since constraint edges have 0 weight

		// For Link(), first param is the leafmost node, 2nd param is the parent
		forest->Link(tar, src);

		SimpleEdge* correspondingEdge = (*boostToSimpleEdgeMap)[e];
		int sortedOrder = correspondingEdge->sortedOrder;
		forest->SetCost(tar, sortedOrder);

		if (SHOW_DEBUG) {
			std::cout << "Tree edge: w:" << sortedOrder << " : " << tar << " (" << g[tar].pt << ") " << src << " (" << g[src].pt << ")" << std::endl;
		}
	}
};

Forest* createLinkCutTree(BoostGraph* g, boost::unordered_map<Edge, SimpleEdge*>* boostToSimpleEdgeMap) {
	Forest* f = new Forest();
	f->Initialize(g->m_vertices.size());

	tree_visitor vis(f, boostToSimpleEdgeMap);

	// Arbitrary root vertex, s
	Vertex s = *(boost::vertices(*g).first);
	boost::breadth_first_search(*g, s, boost::visitor(vis));

	// Because weights are stored in the leafmost node, the node s represents the cost
	// for s and null node (parent of root). This is an invalid edge, so we
	// initialize it with an invalid weight s.t. GetCost(s) is smaller than all possible costs.
	f->SetCost(s, 0);

	return f;
}

EdgeVector* checkCycles(Forest* f, EdgeVector* edgesTPrime, boost::unordered_set<SimpleEdge>* constraintSet, boost::unordered_map<SimpleEdge, SimpleEdge*>* relativeWeightSet) {

	EdgeVector* sEdges = new EdgeVector();

	for (int i = 0; i < edgesTPrime->size(); i++) {
		SimpleEdge* edgeS = (*edgesTPrime)[i];
		VertexIndex u = edgeS->u;
		VertexIndex v = edgeS->v;
		SimpleEdge* cdtEdge = (*relativeWeightSet)[(*edgeS)];
		int we = cdtEdge->sortedOrder;
		assert(we > 0);

		// f->SameEdge(u, v) determines if an edge(u, v) from TPrime already exists in Cmst. Adding an already existing edge to Cmst
		// is guaranteed to not form a cycle and thus is skipped
		if (f->SameEdge(u, v)) {
			continue;
		}

		// f->SameTree(u, v) determines if an edge(u, v) from TPrime belong to the same connected component in Cmst.
		if (f->SameTree(u, v)) {
			Forest::NodeId lca = f->LCA(u, v);

			// LCA weight needs to be ignored (zero weight) because all weights are stored in the leafmost
			// node, i.e. weight on node lca is the edge weight for edge (lca, parent lca)
			int lcaWeight = f->GetCost(lca);
			f->SetCost(lca, 0);

			bool hasParent = f->FindParent(lca) != Forest::UNDEFINED_NODEID;
			Forest::NodeId parentOfLca = Forest::UNDEFINED_NODEID;
			if (hasParent) {
				parentOfLca = f->Cut(lca);
			}

			Forest::NodeId max_u = f->FindMax(u);
			Forest::Cost cost_u = f->GetCost(max_u);
			while (we <= cost_u) {
				f->SetCost(max_u, 0);

				// Handling non-unique costs in the algorithm requires extra work. If there is a cycle C
				// where every edge has equal weight, removing any edge creates an MST. Assume there is an edge e in C that is a constraint.
				// There are a few possible outcomes:
				// 1) The MST algo picks e for TPrime (then e will not exist in S)
				// 2) The MST algo does not pick e for TPrime (then e will still not be added to S since it has equal weight as all other edges)
				// Our solution to this is to check all cycles when they have equal or lesser weight (instead of lesser weight exclusively) and add
				// an edge to S if it originally was a constrained edge
				if (constraintSet->count(SimpleEdge(max_u, f->FindParent(max_u), 0)) > 0) {
					SimpleEdge* edgeS = new SimpleEdge(max_u, f->FindParent(max_u), 0);
					sEdges->push_back(edgeS);
				}

				max_u = f->FindMax(u);
				cost_u = f->GetCost(max_u);
			}

			Forest::NodeId max_v = f->FindMax(v);
			Forest::Cost cost_v = f->GetCost(max_v);
			while (we <= cost_v) {
				f->SetCost(max_v, 0);

				if (constraintSet->count(SimpleEdge(max_v, f->FindParent(max_v), 0)) > 0) {
					SimpleEdge* edgeS = new SimpleEdge(max_v, f->FindParent(max_v), 0);
					sEdges->push_back(edgeS);
				}

				max_v = f->FindMax(v);
				cost_v = f->GetCost(max_v);
			}

			if (hasParent) {
				f->Link(lca, parentOfLca);
			}
			f->SetCost(lca, lcaWeight);
		}
	}

	return sEdges;
}

void computeCmst(
	VertexVector* verticesF,
	EdgeVector* edgesF,
	EdgeVector** NewEdgesF,
	EdgeVector** TPrime,
	EdgeVector** Cmst,
	EdgeVector** S)
{
	// Input: plane forest F = (V, E)
	// Output: minimum set S ⊆ E of constraints such that newF ⊆ CMST(V, S)
	// if (SHOW_DEBUG) { printGraph("Input graph F = (V, E)", F, true); }

	boost::chrono::high_resolution_clock::time_point start;
	boost::chrono::high_resolution_clock::time_point end;
	boost::chrono::milliseconds duration(0);
	boost::chrono::milliseconds total(0);

	// Compute CDT(F)
	start = boost::chrono::high_resolution_clock::now();
	CDT* cdt = computeCdt(verticesF, edgesF);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("CDT(F)", duration);
	if (SHOW_DEBUG) { printCdtInfo(cdt); }

	boost::chrono::milliseconds convertToAdjList(0);

	// Replace F with NewF
	start = boost::chrono::high_resolution_clock::now();
	(*NewEdgesF) = newConstraintSetFromCdt(cdt, verticesF);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	convertToAdjList += duration;
	//printDuration("F -> NewF", duration);
	//if (SHOW_DEBUG) { printGraph("F -> NewF by splitting collinear constraints", *NewF); }

	// CDT -> EdgeVector
	start = boost::chrono::high_resolution_clock::now();
	EdgeVector* cdtEdgeVector = convertCdtToGraph(verticesF, cdt);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	convertToAdjList += duration;
	//printDuration("CDT -> EdgeVector", duration);
	//if (SHOW_DEBUG) { printGraph("F -> NewF by splitting collinear constraints", *NewF); }

	// Recompute sort order (relative weight)
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_map<SimpleEdge, SimpleEdge*>* relativeWeightSet = new boost::unordered_map<SimpleEdge, SimpleEdge*>();
	cdtEdgeVector = sortByWeight(verticesF, cdtEdgeVector);
	// Assign sorted order
	for (int i = 0; i < cdtEdgeVector->size(); i++) {
		SimpleEdge* e = (*cdtEdgeVector)[i];
		e->sortedOrder = i + 1;
		(*relativeWeightSet)[(*e)] = e;
	}
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	convertToAdjList += duration;
	//printDuration("Recompute sort order", duration);
	printDuration("Convert to adj list", convertToAdjList);

	// Compute T' = MST(CDT(NewF)) (Best case MST)
	start = boost::chrono::high_resolution_clock::now();
	(*TPrime) = computeCustomMst(verticesF, cdtEdgeVector);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("T' = MST(CDT(F))", duration);
	//if (SHOW_DEBUG) { printGraph("T' = MST(CDT(F)) Best case MST", *TPrime); }

	// Compute CMST(NewF) = MST(CDT◦(NewF))
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_set<SimpleEdge>* contraintEdgeSet = createSimpleEdgeSet(*NewEdgesF);
	(*Cmst) = computeCustomMst(verticesF, cdtEdgeVector, contraintEdgeSet);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("CMST(F) = MST(CDT◦(F))", duration);
	//if (SHOW_DEBUG) { printGraph("CMST(F) = MST(CDT◦(F))", *Cmst); }

	// Generate Boost graph for CMST
	start = boost::chrono::high_resolution_clock::now();
	boost::unordered_map<Edge, SimpleEdge*>* boostToSimpleEdgeMap;
	BoostGraph* CmstBg = convertVectorToGraph(verticesF, (*Cmst), &boostToSimpleEdgeMap, relativeWeightSet);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("Generate Boost graph for CMST", duration);

	// Create DynamicTree for CMST(NewF)
	start = boost::chrono::high_resolution_clock::now();
	Forest* dt = createLinkCutTree(CmstBg, boostToSimpleEdgeMap);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("Dynamic Tree construction", duration);

	// Check for cycles and construct constraint set s
	start = boost::chrono::high_resolution_clock::now();
	*S = checkCycles(dt, (*TPrime), contraintEdgeSet, relativeWeightSet);
	end = boost::chrono::high_resolution_clock::now();
	duration = (boost::chrono::duration_cast<boost::chrono::milliseconds>(end - start));
	total += duration;
	printDuration("Check cycles", duration);
	//if (SHOW_DEBUG) { printGraph("Contraint set S", *S); }

	printDuration("Total", total);

	delete dt;
	//delete CmstBg;
	delete contraintEdgeSet;
	deleteEdgeVector(cdtEdgeVector);
	delete relativeWeightSet;
	delete cdt;
}

bool containsEdge(boost::unordered_set<SimpleEdge>* edgeSet, SimpleEdge* edge) {
	SimpleEdge se(edge->u, edge->v, 0);
	return edgeSet->count(se) > 0;
}

// True if A a subgraph of B
bool isSubgraph(VertexVector* vertices, EdgeVector* a, EdgeVector* b) {
	boost::unordered_set<SimpleEdge>* bEdgeSet = createSimpleEdgeSet(b);

	bool result = true;

	// Iterate through the edges
	for (int i = 0; i < a->size(); i++) {
		SimpleEdge* edge = (*a)[i];
		if (!containsEdge(bEdgeSet, edge)) {
			CGALPoint* u = (*vertices)[edge->u];
			CGALPoint* v = (*vertices)[edge->v];
			CGAL::Lazy_exact_nt<CGAL::Gmpq> exactWeight = CGAL::squared_distance(*u, *v);
			std::cout << "b contains edge not in a: " << CGAL::to_double(exactWeight) << " (" << *u << ") (" << *v << ")" << std::endl;
			result = false;
		}
	}

	return result;
}

EdgeVector* graphOmitEdge(EdgeVector* edgesS, int omitIndex) {
	EdgeVector* newS = new EdgeVector();
	newS->reserve(edgesS->size());

	int index = 0;
	for (int i = 0; i < edgesS->size(); i++) {
		if (index++ == omitIndex) {
			continue;
		}

		SimpleEdge* edge = (*edgesS)[i];
		SimpleEdge* newEdge = new SimpleEdge(edge->u, edge->v, 0);
		newS->push_back(newEdge);
	}

	return newS;
}

bool isCdtSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	CDT* cdtS = computeCdt(vertices, edgesS); // CDT of mimimum edge constraint
	EdgeVector* ev_cdtS = convertCdtToGraph(vertices, cdtS);
	bool res = isSubgraph(vertices, edgesF, ev_cdtS);

	delete ev_cdtS;
	delete cdtS;

	return res;
}

bool isCmstSubgraph(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	CDT* cdtS = computeCdt(vertices, edgesS); // CDT of mimimum edge constraint
	EdgeVector* ev_cdtS = convertCdtToGraph(vertices, cdtS);
	EdgeVector* cmstS = computeCustomMst(vertices, ev_cdtS, createSimpleEdgeSet(edgesS));
	bool res = isSubgraph(vertices, edgesF, cmstS);

	delete cmstS;
	delete ev_cdtS;
	delete cdtS;

	return res;
}

bool isMinimal(VertexVector* vertices, EdgeVector* edgesF, EdgeVector* edgesS) {
	// Removal of any edge of S should result in F !⊆ CMST(V, S)
	for (int i = 0; i < edgesS->size(); i++) {
		EdgeVector* omittedS = graphOmitEdge(edgesS, i);
		bool sub = isCmstSubgraph(vertices, edgesF, omittedS);
		delete omittedS;

		if (sub) {
			return false;
		}
	}
	return true;
}

bool isContainedIn(EdgeVector* edgesF, EdgeVector* edgesS, EdgeVector* edgesTPrime) {
	boost::unordered_set<SimpleEdge>* sEdgeSet = createSimpleEdgeSet(edgesS);
	boost::unordered_set<SimpleEdge>* tPrimeEdgeSet = createSimpleEdgeSet(edgesTPrime);

	for (int i = 0; i < edgesF->size(); i++) {
		SimpleEdge* edge = (*edgesF)[i];
		if (containsEdge(sEdgeSet, edge))
			continue;
		else if (containsEdge(tPrimeEdgeSet, edge))
			continue;
		else
			return false;
	}

	return true;
}

// TODO:
// Move the PERFORM_RESTRICTION_CHECKS into it's own project
int main(int argc, char* argv[]) {
	const char* vertFile = (argc > 2) ? argv[1] : NULL;
	const char* edgeFile = (argc > 2) ? argv[2] : NULL;

	VertexVector* vertices = NULL;
	EdgeVector* edges = NULL;

	if (vertFile == NULL || edgeFile == NULL) {
		// Random graph
		//createRandomCirclePlaneForest(1000, 1000, 100, &vertices, &edges);
		//createRandomMediumLengthPlaneForest(1000, 1000, 100, &vertices, &edges);
		createRandomNearTriangulation(1000, 1000, 100, &vertices, &edges);
	}
	else {
		// Load graph from file
		// E.g. D:\g\data\HI.nodes D:\g\data\HI.edges
		parseGraph(vertFile, edgeFile, &vertices, &edges);
		//printGDFGraph(vertices, edges);
	}

	/*vertices = new VertexVector();
	vertices->push_back(new CGALPoint(0, 0));
	vertices->push_back(new CGALPoint(1, 0));
	vertices->push_back(new CGALPoint(1, 1));

	edges = new EdgeVector();
	edges->push_back(new SimpleEdge(0, 1, 0));
	edges->push_back(new SimpleEdge(2, 0, 0));*/

	EdgeVector* NewEdges = NULL;
	EdgeVector* TPrime = NULL;
	EdgeVector* Cmst = NULL;
	EdgeVector* S = NULL;

	computeCmst(vertices, edges, &NewEdges, &TPrime, &Cmst, &S);

	//printGDFGraph("D:\\g\\results\\graph examples\\in.gdf", vertices, NewEdges);
	//printGDFGraph("D:\\g\\results\\graph examples\\out.gdf", vertices, S);

	//// Edge Ratio
	//std::cout << "Edges in E: " << NewEdges->size() << " Edges in S: " << S->size() << " Ratio: " << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << S->size() << std::endl;
	std::cout << (double)((double)S->size() / (double)NewEdges->size()) << std::endl;
	std::cout << std::endl;

	// Validatation:
	// S ⊆ E
	if (!isSubgraph(vertices, S, NewEdges)) {
		std::cout << "Error: isSubgraph is false" << std::endl;
	}

	// For each e in NewF, if e not in S, then e in T'
	if (!isContainedIn(NewEdges, S, TPrime)) {
		std::cout << "Error: isContainedIn is false" << std::endl;
	}

	// F ⊆ CDT(V, S) (Note: CMST is contained in CDT)
	if (!isCdtSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCdtSubgraph is false" << std::endl;
	}

	// S ⊆ E s.t. F ⊆ CMST(V, S). Note: CMST(G) = MST(CVG(G)) = MST(CDT◦(G)) (where CDT◦(G) = CDT(G) when all edges in G have 0 weight)
	// Notice: CMST(V, S) is a spanning graph, that is there is at least 1 edge that connects every vertex. So F, the constraint set should be a subset of CMST(V, S)
	// In other words, we want to find the smallest subset S of edges of F such that
	// CMST(F) is equal to CMST(V, S), although the weights of the two trees may
	// be different.
	// NewF ⊆ CMST(V, S)
	if (!isCmstSubgraph(vertices, NewEdges, S)) {
		std::cout << "Error: isCmstSubgraph is false" << std::endl;
	}

	// Removal of any edge of S should result in F !⊆ CMST(V, S) (WARNING: very very slow)
	/*if (!isMinimal(vertices, NewEdges, S)) {
		std::cout << "Error: isMinimal is false" << std::endl;
		}*/

	deleteEdgeVector(S);
	deleteEdgeVector(Cmst);
	deleteEdgeVector(TPrime);
	deleteEdgeVector(NewEdges);
	deleteEdgeVector(edges);
	deleteVerticesVector(vertices);

	return 0;
}


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */
// debugging.cpp - debugging utilities
#include <NTL/xdouble.h>
#include <helib/debugging.h>
#include <helib/norms.h>
#include <helib/Context.h>
#include <helib/Ctxt.h>
#include <helib/EncryptedArray.h>
//#include <helib/powerful.h>

namespace helib {

SecKey* dbgKey = nullptr;
std::shared_ptr<const EncryptedArray> dbgEa = nullptr;
NTL::ZZX dbg_ptxt;

// return the ratio between the real noise <sk,ct> and the estimated one
double realToEstimatedNoise(const Ctxt& ctxt, const SecKey& sk)
{
  NTL::xdouble noiseEst = ctxt.getNoiseBound();
  if (ctxt.isCKKS())
    noiseEst += ctxt.getRatFactor() * ctxt.getPtxtMag();
  NTL::xdouble actualNoise = embeddingLargestCoeff(ctxt, sk);

  return NTL::conv<double>(actualNoise / noiseEst);
}

double log2_realToEstimatedNoise(const Ctxt& ctxt, const SecKey& sk)
{
  NTL::xdouble noiseEst = ctxt.getNoiseBound();
  if (ctxt.isCKKS())
    noiseEst += ctxt.getRatFactor() * ctxt.getPtxtMag();
  NTL::xdouble actualNoise = embeddingLargestCoeff(ctxt, sk);

  return log(actualNoise / noiseEst) / log(2.0);
}

// check that real-to-estimated ratio is not too large, print warning otherwise
void checkNoise(const Ctxt& ctxt,
                const SecKey& sk,
                const std::string& msg,
                double thresh)
{
  double ratio;
  if ((ratio = realToEstimatedNoise(ctxt, sk)) > thresh) {
    std::cerr << "\n*** too much noise: " << msg << ": " << ratio << "\n";
  }
}

// Decrypt and find the l-infinity norm of the result in canonical embedding
NTL::xdouble embeddingLargestCoeff(const Ctxt& ctxt, const SecKey& sk)
{
  const Context& context = ctxt.getContext();
  NTL::ZZX p, pp;
  sk.Decrypt(p, ctxt, pp);
  return embeddingLargestCoeff(pp, context.zMStar);
}

void decryptAndPrint(std::ostream& s,
                     const Ctxt& ctxt,
                     const SecKey& sk,
                     const EncryptedArray& ea,
                     long flags)
{
  const Context& context = ctxt.getContext();
  std::vector<NTL::ZZX> ptxt;
  NTL::ZZX p, pp;
  sk.Decrypt(p, ctxt, pp);

  NTL::xdouble modulus = NTL::xexp(context.logOfProduct(ctxt.getPrimeSet()));
  NTL::xdouble actualNoise =
      embeddingLargestCoeff(pp, ctxt.getContext().zMStar);
  NTL::xdouble noiseEst = ctxt.getNoiseBound();
  if (ctxt.isCKKS())
    noiseEst += ctxt.getRatFactor() * ctxt.getPtxtMag();

  s << "plaintext space mod " << ctxt.getPtxtSpace()
    << ", bitCapacity=" << ctxt.bitCapacity() << ", \n           |noise|=q*"
    << (actualNoise / modulus) << ", |noiseBound|=q*" << (noiseEst / modulus);
  if (ctxt.isCKKS()) {
    s << ", \n           ratFactor=" << ctxt.getRatFactor()
      << ", ptxtMag=" << ctxt.getPtxtMag()
      << ", realMag=" << (actualNoise / ctxt.getRatFactor());
  }
  s << std::endl;

  if (flags & FLAG_PRINT_ZZX) {
    s << "   before mod-p reduction=";
    printZZX(s, pp) << std::endl;
  }
  if (flags & FLAG_PRINT_POLY) {
    s << "   after mod-p reduction=";
    printZZX(s, p) << std::endl;
  }
  if (flags & FLAG_PRINT_VEC) { // decode to a vector of ZZX
    ea.decode(ptxt, p);
    if (ea.getAlMod().getTag() == PA_zz_p_tag &&
        ctxt.getPtxtSpace() != ea.getAlMod().getPPowR()) {
      long g = NTL::GCD(ctxt.getPtxtSpace(), ea.getAlMod().getPPowR());
      for (long i = 0; i < ea.size(); i++)
        PolyRed(ptxt[i], g, true);
    }
    s << "   decoded to ";
    if (deg(p) < 40) // just pring the whole thing
      s << ptxt << std::endl;
    else if (ptxt.size() == 1) // a single slot
      printZZX(s, ptxt[0]) << std::endl;
    else { // print first and last slots
      printZZX(s, ptxt[0], 20) << "--";
      printZZX(s, ptxt[ptxt.size() - 1], 20) << std::endl;
    }
  } else if (flags & FLAG_PRINT_DVEC) { // decode to a vector of doubles
    const EncryptedArrayCx& eacx = ea.getCx();
    std::vector<double> v;
    eacx.decrypt(ctxt, sk, v);
    printVec(s << "           ", v, 20) << std::endl;
  } else if (flags & FLAG_PRINT_XVEC) { // decode to a vector of complex
    const EncryptedArrayCx& eacx = ea.getCx();
    std::vector<cx_double> v;
    eacx.decrypt(ctxt, sk, v);
    printVec(s << "           ", v, 20) << std::endl;
  }
}

bool decryptAndCompare(const Ctxt& ctxt,
                       const SecKey& sk,
                       const EncryptedArray& ea,
                       const PlaintextArray& pa)
{
  PlaintextArray ppa(ea);
  ea.decrypt(ctxt, sk, ppa);

  return equals(ea, pa, ppa);
}

// Compute decryption with.without mod-q on a vector of ZZX'es,
// useful when debugging bootstrapping (after "raw mod-switch")
void rawDecrypt(NTL::ZZX& plaintxt,
                const std::vector<NTL::ZZX>& zzParts,
                const DoubleCRT& sKey,
                long q)
{
  const Context& context = sKey.getContext();

  // Set to zzParts[0] + sKey * zzParts[1] "over the integers"
  DoubleCRT ptxt = sKey;
  ptxt *= zzParts[1];
  ptxt += zzParts[0];

  // convert to coefficient representation
  ptxt.toPoly(plaintxt);

  if (q > 1)
    PolyRed(plaintxt, q, false /*reduce to [-q/2,1/2]*/);
}

void CheckCtxt(const Ctxt& c, const char* label)
{
  std::cerr << "  " << label
            << ", log2(modulus/noise)=" << (-c.log_of_ratio() / log(2.0))
            << ", p^r=" << c.getPtxtSpace();

  if (dbgKey) {
    double ratio = log2_realToEstimatedNoise(c, *dbgKey);
    std::cerr << ", log2(noise/bound)=" << ratio;
    if (ratio > 0)
      std::cerr << " BAD-BOUND";
  }

#if 0
  // This is not really a useful test

  if (dbgKey && c.getContext().isBootstrappable()) {
    Ctxt c1(c);
    //c1.dropSmallAndSpecialPrimes();

    const Context& context = c1.getContext();
    const RecryptData& rcData = context.rcData;
    const PAlgebra& palg = context.zMStar;

    NTL::ZZX p, pp;
    dbgKey->Decrypt(p, c1, pp);
    NTL::Vec<NTL::ZZ> powerful;
    rcData.p2dConv->ZZXtoPowerful(powerful, pp);

    NTL::ZZ q;
    q = context.productOfPrimes(c1.getPrimeSet());
    vecRed(powerful, powerful, q, false);

    NTL::ZZX pp_alt;
    rcData.p2dConv->powerfulToZZX(pp_alt, powerful);

    NTL::xdouble max_coeff = NTL::conv<NTL::xdouble>(largestCoeff(pp));
    NTL::xdouble max_pwrfl = NTL::conv<NTL::xdouble>(largestCoeff(powerful));
    NTL::xdouble max_canon = embeddingLargestCoeff(pp_alt, palg);
    double ratio = log(max_pwrfl/max_canon)/log(2.0);

    //cerr << ", max_coeff=" << max_coeff;
    //cerr << ", max_pwrfl=" << max_pwrfl;
    //cerr << ", max_canon=" << max_canon;
    std::cerr << ", log2(max_pwrfl/max_canon)=" << ratio;
    if (ratio > 0) std::cerr << " BAD-BOUND";
  }
#endif

  std::cerr << std::endl;
}

} // namespace helib


#include "rave_to_rbdl.hpp"
#include <map>
#include <boost/foreach.hpp>
#include "trajopt/utils.hpp"
#include "trajopt/rave_utils.hpp"
using namespace OpenRAVE;
using namespace Eigen;
using namespace trajopt;
using namespace std;
using namespace util;

namespace trajopt {

Matrix3d toMatrix3d(const OpenRAVE::TransformMatrix& tm) {
  Matrix3d out;
  for (int i=0; i < 3; ++i) for (int j=0; j < 3; ++j) out(i,j) = tm.rot(i,j);
  return out;
}

rbdmath::SpatialTransform toSpatialTransform(const OpenRAVE::Transform& T) {
  return rbdmath::SpatialTransform(toMatrix3d(T).transpose(), toVector3d(T.trans));
}

OpenRAVE::Transform toRave(const rbdmath::SpatialTransform& T) {
  OpenRAVE::TransformMatrix tm;
  tm.trans = toRave(T.r);
  for (int i=0; i < 3; ++i)
    for (int j=0; j < 3; ++j)
      tm.rot(i,j) = T.E(j,i);
  return OpenRAVE::Transform(tm);
}

boost::shared_ptr<rbd::Model> MakeRBDLModel(OpenRAVE::RobotBasePtr robot, bool floating_base, Link2ID& link2id) {
  boost::shared_ptr<rbd::Model> model(new rbd::Model());
  model->Init();
  model->gravity = Vector3d (0., 0, -9.81);

  KinBody::LinkPtr base_link = robot->GetJoints().empty() ? robot->GetLinks()[0] : robot->GetJoints()[0]->GetHierarchyParentLink();
  unsigned base_id = 0;
  if (floating_base) base_id = model->SetFloatingBaseBody(rbd::Body(base_link->GetMass(), toVector3d(base_link->GetLocalCOM()), toMatrix3d(base_link->GetLocalInertia())));

  printf("base id: %i\n",base_id);
  map<KinBody::LinkPtr, OpenRAVE::Transform> link2Tjl;

  link2id[base_link] = base_id;
  link2Tjl[base_link] = robot->GetTransform().inverse() * base_link->GetTransform(); // T_robot_base

  BOOST_FOREACH(const KinBody::JointPtr& joint, robot->GetJoints()) {

    KinBody::LinkPtr parent = joint->GetHierarchyParentLink(),
                    child = joint->GetHierarchyChildLink();
    KinBody::JointType rave_type = joint->GetType();
    rbd::JointType rbd_type = (rave_type == KinBody::JointRevolute) ? rbd::JointTypeRevolute : rbd::JointTypePrismatic;
    rbd::Joint rbd_joint(rbd_type, toVector3d(joint->GetInternalHierarchyAxis()));

    OpenRAVE::Transform T_l0_j = joint->GetInternalHierarchyLeftTransform(),
                        T_j_l1 = joint->GetInternalHierarchyRightTransform();

    Matrix3d T_j_m = toMatrix3d(T_j_l1 * /* T_l1_m */ child->GetLocalMassFrame());
    Matrix3d inertia_mm = toMatrix3d(child->GetLocalInertia());
    Matrix3d inertia_jj = T_j_m.inverse() * inertia_mm * T_j_m;

    rbd::Body body(child->GetMass(), toVector3d(T_j_l1 * child->GetLocalCOM()), inertia_jj);
    cout << "----------" << endl;
    cout <<  joint->GetName() << " " << rave_type << endl;

    KinBody::LinkPtr realParent = parent;
    while (link2id.find(realParent) == link2id.end()) {
      cout << "realParent" << realParent->GetName() << endl;
      vector<KinBody::LinkPtr> parentLinks;
      realParent->GetParentLinks(parentLinks);
      assert(parentLinks.size() == 1);
      realParent = parentLinks[0];
    }

    cout << "parent: " << parent->GetName() << " " << link2id[parent] << endl;
    link2id[child] = model->AddBody(link2id[realParent], toSpatialTransform(link2Tjl[realParent] * T_l0_j), rbd_joint, body, child->GetName());
    cout << "child: " << child->GetName() << link2id[child] << endl;
    link2Tjl[child] = T_j_l1;
    cout << "---------" << endl;
  }

  return model;

}

}


/**
 * @file layer_traits_test.cpp
 * @author Marcus Edel
 *
 * Test the LayerTraits class. Because all of the values are known at compile
 * time, this test is meant to ensure that uses of LayerTraits still compile
 * okay and react as expected.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer_traits.hpp>
#include <mlpack/methods/ann/layer/bias_layer.hpp>
#include <mlpack/methods/ann/layer/multiclass_classification_layer.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(LayerTraitsTest);

// Test the defaults.
BOOST_AUTO_TEST_CASE(DefaultsTraitsTest)
{
  // An irrelevant non-connection type class is used here so that the default
  // implementation of ConnectionTraits is chosen.
  bool b = LayerTraits<int>::IsBinary;
  BOOST_REQUIRE_EQUAL(b, false);

  b =  LayerTraits<int>::IsOutputLayer;
  BOOST_REQUIRE_EQUAL(b, false);

  b =  LayerTraits<int>::IsBiasLayer;
  BOOST_REQUIRE_EQUAL(b, false);
}

// Test the BiasLayer traits.
BOOST_AUTO_TEST_CASE(BiasLayerTraitsTest)
{
  bool b = LayerTraits<BiasLayer<> >::IsBinary;
  BOOST_REQUIRE_EQUAL(b, false);

  b = LayerTraits<BiasLayer<> >::IsOutputLayer;
  BOOST_REQUIRE_EQUAL(b, false);

  b = LayerTraits<BiasLayer<> >::IsBiasLayer;
  BOOST_REQUIRE_EQUAL(b, true);
}

// Test the MulticlassClassificationLayer traits.
BOOST_AUTO_TEST_CASE(MulticlassClassificationLayerTraitsTest)
{
  bool b = LayerTraits<MulticlassClassificationLayer<> >::IsBinary;
  BOOST_REQUIRE_EQUAL(b, false);

  b = LayerTraits<MulticlassClassificationLayer<> >::IsOutputLayer;
  BOOST_REQUIRE_EQUAL(b, true);

  b = LayerTraits<MulticlassClassificationLayer<> >::IsBiasLayer;
  BOOST_REQUIRE_EQUAL(b, false);
}

BOOST_AUTO_TEST_SUITE_END();


/*
    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 "itkio.h"
#include "miaImageProcessing.h"

#include <cstdio>
#include <cmath>

#include <vector>
#include <string>
#include <sstream> //for ostringstream.
#include <fstream>
#include "densecrf.h"

#include "ioparsing.hpp"
#include "parameters.h"
#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>
#include <chrono>

namespace po = boost::program_options;
namespace fs = boost::filesystem;

using namespace mia;

std::vector<Image> load_multi_channel_image(std::vector<std::string> image_filenames)
{

	std::vector<Image> images;
	images.push_back(itkio::load(image_filenames[0]));

	for (int c = 1; c < image_filenames.size(); c++)
	{
		Image img = itkio::load(image_filenames[c]);
		if (img.size() != images[0].size())
		{
			std::cout << "Image modalities size mismatch! Aborting..." << std::endl;
			exit(0);
		}
		else
			images.push_back(img);
	}
	return images;
}

float cap_prob(float p)
{

	return std::fmax(0.05, std::fmin(p, 0.95));
}

//Loads the nii with the unary potentials and makes a matrix that the rest of the original code needs.
void setupUnaryEnergyMatrix(MatrixXf &unaryCosts, std::vector<Image> probmaps, int numberOfForegroundClasses, bool computeBackgroundProbmap = true)
{

	int numberOfVoxelsInImage = probmaps[0].size();
	int start_class_index = 0;
	int numberOfClasses = numberOfForegroundClasses + 1;
	if (computeBackgroundProbmap)
		start_class_index = 1;

	float eps = 0.001;
	unaryCosts.setZero();
	for (int c = 0; c < numberOfForegroundClasses; ++c)
	{

		int index = 0;
		for (int z = 0; z < probmaps[c].sizeZ(); z++)
		{
			for (int y = 0; y < probmaps[c].sizeY(); y++)
			{
				for (int x = 0; x < probmaps[c].sizeX(); x++)
				{
					float val = probmaps[c](x, y, z);

					val = -log(cap_prob(probmaps[c](x, y, z)));
					unaryCosts(c + start_class_index, index) = val;
					if (computeBackgroundProbmap)
						unaryCosts(0, index) += std::fmax(0, probmaps[c](x, y, z));
					index++;
				}
			}
		}
	}
	if (computeBackgroundProbmap)
	{
		int index = 0;
		for (int z = 0; z < probmaps[0].sizeZ(); z++)
		{
			for (int y = 0; y < probmaps[0].sizeY(); y++)
			{
				for (int x = 0; x < probmaps[0].sizeX(); x++)
				{

					unaryCosts(0, index) = -log(cap_prob(1 - unaryCosts(0, index)));
					index++;
				}
			}
		}
	}
}

void addPairwiseBilateralMultiMod(DenseCRF3D &crf3d, std::vector<float> sXYZ, std::vector<float> sMod, std::vector<Image> &images, LabelCompatibility *function = NULL, KernelType kernel_type = DIAG_KERNEL, NormalizationType normalization_type = NORMALIZE_SYMMETRIC)
{
	int numImages = images.size();
	int sizeX = images[0].sizeX();
	int sizeY = images[0].sizeY();
	int sizeZ = images[0].sizeZ();

	MatrixXf feature(3 + numImages, images[0].size());
	feature.setZero();
	int index = 0;
	for (int z = 0; z < images[0].sizeZ(); z++)
	{
		for (int y = 0; y < images[0].sizeY(); y++)
		{
			for (int x = 0; x < images[0].sizeX(); x++)
			{
				feature(0, index) = x / sXYZ[0];
				feature(1, index) = y / sXYZ[1];
				feature(2, index) = z / sXYZ[2];

				for (int imgIdx = 0; imgIdx < numImages; imgIdx++)
					feature(3 + imgIdx, index) = images[imgIdx](x, y, z) / sMod[imgIdx];

				index++;
			}
		}
	}
	crf3d.addPairwiseEnergy(feature, function, kernel_type, normalization_type);
}

int main(int argc, char *argv[])
{

	Parameters params;

	std::string config_file;

	std::vector<std::string> simages;
	std::vector<std::string> simagelists;
	std::vector<std::string> sprobmaps;
	std::vector<std::string> sprobmaplists;

	std::string output_path = ".";

	int numberOfModalities = 0;
	int numberOfForegroundClasses = 0;
	bool computeBackgroundProbmap = true;
	bool overwrite_output = false;
	bool do_output_probmaps = false;
	bool do_output_unary = false;
	std::vector<std::string> sbilateralXYZStds;
	std::vector<std::string> sposXYZStds;
	std::vector<std::string> sbilateralModStds;

	float minIntensity = -3, maxIntensity = +3;

	//====================================

	try
	{
		// Declare the supported options.
		po::options_description generic("generic options");
		generic.add_options()("help", "produce help message")("config", po::value<std::string>(&config_file), "configuration file")("probs", po::bool_switch(&do_output_probmaps)->default_value(false), "")("unary", po::bool_switch(&do_output_unary)->default_value(false), "")("overwrite", po::bool_switch(&overwrite_output)->default_value(false), "");

		po::options_description config("specific options");
		config.add_options()("output", po::value<std::string>(&output_path), "output path")("image", po::value<std::vector<std::string>>(&simages)->multitoken(), "filename(s) of multi-modality images")("imagelist", po::value<std::vector<std::string>>(&simagelists)->multitoken(), "text file(s) listing images")("probmap", po::value<std::vector<std::string>>(&sprobmaps)->multitoken(), "filename(s) of multi-modality images")("probmaplist", po::value<std::vector<std::string>>(&sprobmaplists)->multitoken(), "text file(s) listing images")("posXYZStds", po::value<std::vector<std::string>>(&sposXYZStds), "posXYZStds")("biModStds", po::value<std::vector<std::string>>(&sbilateralModStds), "bilateralModStds")("biXYZStds", po::value<std::vector<std::string>>(&sbilateralXYZStds), "bilateralXYZStds")("numberOfForegroundClasses", po::value<int>(&numberOfForegroundClasses), "numberOfForegroundClasses")("posW", po::value<float>(&params.posW), "posW")("biW", po::value<float>(&params.bilateralW), "biW")("maxIterations", po::value<int>(&params.maxIterations), "maxIteration");

		po::options_description cmdline_options("options");
		cmdline_options.add(generic).add(config);

		po::options_description config_file_options;
		config_file_options.add(config);

		po::variables_map vm;

		po::store(po::parse_command_line(argc, argv, cmdline_options), vm);
		po::notify(vm);

		if (vm.count("config"))
		{
			std::ifstream ifs(config_file.c_str());
			if (!ifs)
			{
				std::cout << "cannot open config file: " << config_file << std::endl;
				return 0;
			}
			else
			{
				po::store(parse_config_file(ifs, config_file_options), vm);
				po::notify(vm);
			}
		}

		if (vm.count("help"))
		{
			std::cout << cmdline_options << std::endl;
			return 0;
		}
	}
	catch (std::exception &e)
	{
		std::cout << e.what() << std::endl;
		return 1;
	}

	if (!fs::exists(output_path)) fs::create_directories(output_path);

	//================================

	std::vector<std::string> images;
	strings_to_values(simages, images);

	std::vector<std::string> imagelists;
	strings_to_values(simagelists, imagelists);

	strings_to_values(sbilateralXYZStds, params.bilateralXYZStds, true);

	strings_to_values(sposXYZStds, params.posXYZStds, true);

	std::vector<std::string> probmaps;
	strings_to_values(sprobmaps, probmaps);

	std::vector<std::string> probmaplists;
	strings_to_values(sprobmaplists, probmaplists);

	std::vector<std::vector<std::string>> image_filenames = parse_multichannel_filepaths(images, imagelists);
	std::vector<std::vector<std::string>> probmap_filenames = parse_multichannel_filepaths(probmaps, probmaplists);

	int numberOfImages = image_filenames.size();
	int numberOfProbChannels = probmap_filenames.size();

	if (numberOfImages == 0)
	{
		std::cout << "No images provided. Aborting..." << std::endl;
	}
	if (numberOfProbChannels == 0)
	{
		std::cout << "No probmaps provided. Aborting..." << std::endl;
		exit(EXIT_FAILURE);
	}

	if (numberOfProbChannels == numberOfForegroundClasses)
		computeBackgroundProbmap = true;
	else if (numberOfForegroundClasses > 0 && numberOfProbChannels == numberOfForegroundClasses + 1)
		computeBackgroundProbmap = false;
	else
	{
		std::cout << "number of probmaps mismatches with provided number of foreground classes" << std::endl;
		exit(EXIT_FAILURE);
	}

	numberOfModalities = image_filenames[0].size();
	int numClasses = numberOfForegroundClasses + 1;

	params.setNumberOfModalities(numberOfModalities);
	strings_to_values(sbilateralModStds, params.bilateralModsStds, true);

	std::vector<std::vector<Image>> intensity_images;

	params.print();

	for (int imgIdx = 0; imgIdx < numberOfImages; imgIdx++)
	{

		fs::path input_path(image_filenames[imgIdx][0]);

		std::string basename = fs::basename(input_path);
		if (fs::extension(basename) != "")
			basename = fs::basename(basename);
		std::stringstream base_output_sfilename;
		std::stringstream labelmap_output_path;

		base_output_sfilename << output_path << "/" << basename;
		labelmap_output_path << base_output_sfilename.str() << "_labelmap.nii.gz";

		if (fs::exists(labelmap_output_path.str()))
		{
			if (!overwrite_output)
			{
				std::cout << labelmap_output_path.str() << " already exists. skipping." << std::endl;
				continue;
			}
			else
				std::cout << labelmap_output_path.str() << " already exists. Overwriting." << std::endl;
		}
		std::cout << labelmap_output_path.str() << std::endl;

		namespace ch = std::chrono;
		auto start = ch::high_resolution_clock::now();

		std::cout << "loading data " << imgIdx + 1 << " of " << image_filenames.size() << " with " << numberOfModalities << " channel(s)" << std::endl;

		std::vector<Image> multiChannelImage = load_multi_channel_image(image_filenames[imgIdx]);
		std::vector<Image> imageProbmaps = load_multi_channel_image(probmap_filenames[imgIdx]);

		int sizeX = multiChannelImage[0].sizeX();
		int sizeY = multiChannelImage[0].sizeY();
		int sizeZ = multiChannelImage[0].sizeZ();

		DenseCRF3D crf3d(sizeX, sizeY, sizeZ, numberOfForegroundClasses + 1);

		std::cout << "********** Setting up Unary. **********" << std::endl;
		MatrixXf unaryCost = MatrixXf(numClasses, multiChannelImage[0].size());
		setupUnaryEnergyMatrix(unaryCost, imageProbmaps, numberOfForegroundClasses, computeBackgroundProbmap);
		crf3d.setUnaryEnergy(unaryCost);

		std::cout << "********** Setting up Pairwise potentials **********" << std::endl;
		crf3d.addPairwiseGaussian(params.posXYZStds[0],
								  params.posXYZStds[1],
								  params.posXYZStds[2],
								  new PottsCompatibility(params.posW));

		addPairwiseBilateralMultiMod(crf3d,
									 params.bilateralXYZStds,
									 params.bilateralModsStds,
									 multiChannelImage,
									 new PottsCompatibility(params.bilateralW));

		std::cout << "++++++++++++++++++++++++++ Performing Inference ++++++++++++++++++++++++++" << std::endl;

		MatrixXf probMapsMatrix = crf3d.inference(params.maxIterations);

		if (do_output_probmaps)
		{
			Image output_probmap = imageProbmaps[0].clone();
			for (int i = 0; i < numClasses; ++i)
			{
				std::stringstream output_filename;
				output_filename << base_output_sfilename.str() << "_probmap" << i << ".nii.gz";
				int index = 0;
				for (int z = 0; z < imageProbmaps[0].sizeZ(); z++)
				{
					for (int y = 0; y < imageProbmaps[0].sizeY(); y++)
					{
						for (int x = 0; x < imageProbmaps[0].sizeX(); x++)
						{
							output_probmap(x, y, z) = probMapsMatrix(i, index++);
						}
					}
				}
				itkio::save(output_probmap, output_filename.str());
			}
		}

		if (do_output_unary)
		{
			Image output_probmap = imageProbmaps[0].clone();
			for (int i = 0; i < numClasses; ++i)
			{
				std::stringstream output_filename;
				output_filename << base_output_sfilename.str() << "_unary" << i << ".nii.gz";
				int index = 0;
				for (int z = 0; z < imageProbmaps[0].sizeZ(); z++)
				{
					for (int y = 0; y < imageProbmaps[0].sizeY(); y++)
					{
						for (int x = 0; x < imageProbmaps[0].sizeX(); x++)
						{
							output_probmap(x, y, z) = unaryCost(i, index++);
						}
					}
				}
				itkio::save(output_probmap, output_filename.str());
			}
		}

		VectorXs segmentationVector = crf3d.currentMap(probMapsMatrix);

		Image output_labelmap = imageProbmaps[0].clone();
		output_labelmap.dataType(mia::USHORT);
		int index = 0;
		for (int z = 0; z < imageProbmaps[0].sizeZ(); z++)
		{
			for (int y = 0; y < imageProbmaps[0].sizeY(); y++)
			{
				for (int x = 0; x < imageProbmaps[0].sizeX(); x++)
				{
					output_labelmap(x, y, z) = segmentationVector[index++];
				}
			}
		}

		itkio::save(output_labelmap, labelmap_output_path.str());

		std::cout << "++++++++++++++++++++++++++ Done. ++++++++++++++++++++++++++" << std::endl;
		auto stop = ch::high_resolution_clock::now();
		std::cout << "done. took " << ch::duration_cast<ch::milliseconds>(stop - start).count() << " ms" << std::endl;
	}
}


#include <sm/eigen/gtest.hpp>
#include <aslam/backend/OptimizerRprop.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <sm/random.hpp>
#include <aslam/backend/test/ErrorTermTester.hpp>
#include <aslam/backend/test/SampleDvAndError.hpp>

TEST(OptimizerRpropTestSuite, testRpropNonSquaredErrorTerms)
{
  try {
    using namespace aslam::backend;
    boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
    OptimizationProblem& problem = *problem_ptr;

    const int P = 2;
    const int E = 3;
    // Add some design variables.
    std::vector< boost::shared_ptr<Point2d> > p2d;
    p2d.reserve(P);
    for (int p = 0; p < P; ++p) {
      boost::shared_ptr<Point2d> point(new Point2d(Eigen::Vector2d::Random())); // random initialization of design variable
      p2d.push_back(point);
      problem.addDesignVariable(point);
      point->setBlockIndex(p);
      point->setActive(true);
    }

    // make a deep copy
    std::vector< boost::shared_ptr<Point2d> > p2d0;
    p2d0.reserve(p2d.size());
    for (auto& dv : p2d) p2d0.emplace_back(new Point2d(*dv));

    // Add some error terms.
    std::vector< boost::shared_ptr<TestNonSquaredError> > e1;
    e1.reserve(P*E);
    for (int p = 0; p < P; ++p) {
      for (int e = 0; e < E; ++e) {
        TestNonSquaredError::grad_t g(p+1, e+1);
        boost::shared_ptr<TestNonSquaredError> err(new TestNonSquaredError(p2d[p].get(), g));
        err->_p = 1.0;
        e1.push_back(err);
        problem.addErrorTerm(err);
        SCOPED_TRACE("");
        testErrorTerm(err);
      }
    }
    // Now let's optimize.
    OptimizerRprop::Options options;
    options.maxIterations = 500;
    options.numThreadsJacobian = 8;
    options.convergenceGradientNorm = 0.0;
    options.convergenceDeltaX = 0.0;
    EXPECT_ANY_THROW(options.check());
    options.convergenceGradientNorm = 1e-6;
    EXPECT_NO_THROW(options.check());
    OptimizerRprop optimizer(options);
    optimizer.setProblem(problem_ptr);

    EXPECT_NO_THROW(optimizer.checkProblemSetup());

    for (OptimizerRprop::Options::Method method : {OptimizerRprop::Options::RPROP_PLUS, OptimizerRprop::Options::RPROP_MINUS,
      OptimizerRprop::Options::IRPROP_MINUS, OptimizerRprop::Options::IRPROP_PLUS}) {

      optimizer.getOptions().method = method;
      optimizer.initialize();
      for (std::size_t i=0; i<p2d.size(); i++) p2d[i]->_v = p2d0[i]->_v;
      SCOPED_TRACE("");
      optimizer.optimize();
      auto ret = optimizer.getStatus();
      EXPECT_TRUE(ret.success());
      EXPECT_LE(ret.gradientNorm, 1e-6);
      EXPECT_GE(ret.error, 0.0);
      EXPECT_LT(ret.maxDeltaX, 1e-3);
      if (method == OptimizerRprop::Options::IRPROP_PLUS)
        EXPECT_LT(ret.deltaError, 1e-12);
    }

  } catch (const std::exception& e) {
    FAIL() << e.what();
  }
}

TEST(OptimizerRpropTestSuite, testRpropSquaredErrorTerms)
{
  try {
    using namespace aslam::backend;
    boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
    OptimizationProblem& problem = *problem_ptr;
    const int P = 2;
    const int E = 3;
    // Add some design variables.
    std::vector< boost::shared_ptr<Point2d> > p2d;
    p2d.reserve(P);
    for (int p = 0; p < P; ++p) {
      boost::shared_ptr<Point2d> point(new Point2d(Eigen::Vector2d::Random())); // random initialization of design variable
      p2d.push_back(point);
      problem.addDesignVariable(point);
      point->setBlockIndex(p);
      point->setActive(true);
    }

    // make a deep copy
    std::vector< boost::shared_ptr<Point2d> > p2d0;
    p2d0.reserve(p2d.size());
    for (auto& dv : p2d) p2d0.emplace_back(new Point2d(*dv));

    // Add some error terms.
    std::vector< boost::shared_ptr<LinearErr> > e1;
    e1.reserve(P*E);
    for (int p = 0; p < P; ++p) {
      for (int e = 0; e < E; ++e) {
        boost::shared_ptr<LinearErr> err(new LinearErr(p2d[p].get()));
        e1.push_back(err);
        problem.addErrorTerm(err);
        SCOPED_TRACE("");
        testErrorTerm(err);
      }
    }
    // Now let's optimize.
    OptimizerRprop::Options options;
    options.method = OptimizerRprop::Options::RPROP_PLUS;
    options.maxIterations = 500;
    options.numThreadsJacobian = 8;
    OptimizerRprop optimizer(options);
    optimizer.setProblem(problem_ptr);

    EXPECT_NO_THROW(optimizer.checkProblemSetup());

    for (OptimizerRprop::Options::Method method : {OptimizerRprop::Options::RPROP_PLUS, OptimizerRprop::Options::RPROP_MINUS,
      OptimizerRprop::Options::IRPROP_MINUS, OptimizerRprop::Options::IRPROP_PLUS}) {

      optimizer.getOptions().method = method;
      optimizer.initialize();
      for (std::size_t i=0; i<p2d.size(); i++) p2d[i]->_v = p2d0[i]->_v;
      SCOPED_TRACE("");
      optimizer.optimize();
      auto ret = optimizer.getStatus();
      EXPECT_TRUE(ret.success());
      EXPECT_LT(ret.gradientNorm, 1e-3);
      EXPECT_GE(ret.error, 0.0);
      EXPECT_LT(ret.maxDeltaX, 1e-3);
      if (method == OptimizerRprop::Options::IRPROP_PLUS)
        EXPECT_LT(ret.deltaError, 1e-12);
    }

  } catch (const std::exception& e) {
    FAIL() << e.what();
  }
}