//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// 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)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_BLOOM_FILTER_HPP
#define BOOST_BLOOM_FILTER_BLOOM_FILTER_HPP 1

#include <cmath>
#include <bitset>

#include <boost/config.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/size.hpp>

#include <boost/bloom_filter/detail/apply_hash.hpp>
#include <boost/bloom_filter/hash/default.hpp>

#ifndef BOOST_NO_0X_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif

namespace boost {
  namespace bloom_filters {
    template <typename T,
	      size_t Size,
	      class HashFunctions = mpl::vector<boost_hash<T> > >
    class basic_bloom_filter {
    public:
      typedef T value_type;
      typedef T key_type;
      typedef std::bitset<Size> bitset_type;
      typedef HashFunctions hash_function_type;
      typedef basic_bloom_filter<T, Size,
				 HashFunctions> this_type;

    private:
      typedef detail::apply_hash<mpl::size<HashFunctions>::value - 1,
				 this_type> apply_hash_type;

    public:
      basic_bloom_filter() {}

      template <typename InputIterator>
      basic_bloom_filter(const InputIterator start, const InputIterator end) {
	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

#ifndef BOOST_NO_0X_HDR_INITIALIZER_LIST
      basic_bloom_filter(const std::initializer_list<T>& ilist) {
	typedef typename std::initializer_list<T>::const_iterator citer;
	for (citer i = ilist.begin(), end = ilist.end(); i != end; ++i) {
	  this->insert(*i);
	}
      }
#endif

      static BOOST_CONSTEXPR size_t bit_capacity() {
        return Size;
      }

      static BOOST_CONSTEXPR size_t num_hash_functions() {
        return mpl::size<HashFunctions>::value;
      };

      double false_positive_rate() const {
        const double n = static_cast<double>(this->bits.count());
        static const double k = static_cast<double>(num_hash_functions());
        static const double m = static_cast<double>(Size);
        static const double e =
	  2.718281828459045235360287471352662497757247093699959574966;
        return std::pow(1 - std::pow(e, -k * n / m), k);
      };

      size_t count() const {
        return this->bits.count();
      };

      bool empty() const {
	return this->count() == 0;
      }

      const bitset_type&
      data() const
      {
	return this->bits;
      }

      void insert(const T& t) {
        apply_hash_type::insert(t, bits);
      }

      template <typename InputIterator>
      void insert(const InputIterator start, const InputIterator end) {
	for (InputIterator i = start; i != end; ++i) {
	  this->insert(*i);
	}
      }

      bool probably_contains(const T& t) const {
        return apply_hash_type::contains(t, bits);
      }

      void clear() {
        this->bits.reset();
      }

      void swap(basic_bloom_filter& other) {
	basic_bloom_filter tmp = other;
	other = *this;
	*this = tmp;
      }

      basic_bloom_filter& operator|=(const basic_bloom_filter& rhs) {
        this->bits |= rhs.bits;
        return *this;
      }

      basic_bloom_filter& operator&=(const basic_bloom_filter& rhs) {
        this->bits &= rhs.bits;
        return *this;
      }

      template<class _T, size_t _Size, class _HashFunctions>
      friend bool
      operator==(const basic_bloom_filter<_T, _Size, _HashFunctions>&,
		 const basic_bloom_filter<_T, _Size, _HashFunctions>&);

      template<class _T, size_t _Size, class _HashFunctions>
      friend bool
      operator!=(const basic_bloom_filter<_T, _Size, _HashFunctions>&,
		 const basic_bloom_filter<_T, _Size, _HashFunctions>&);

    private:
      bitset_type bits;
    };

    template<class _T, size_t _Size, class _HashFunctions>
    bool
    operator==(const basic_bloom_filter<_T, _Size, _HashFunctions>& lhs,
	       const basic_bloom_filter<_T, _Size, _HashFunctions>& rhs)
    {
      return (lhs.bits == rhs.bits);
    }

    template<class _T, size_t _Size, class _HashFunctions>
    bool
    operator!=(const basic_bloom_filter<_T, _Size, _HashFunctions>& lhs,
	       const basic_bloom_filter<_T, _Size, _HashFunctions>& rhs)
    {
      return !(lhs == rhs);
    }

    template<class _T, size_t _Size, class _HashFunctions>
    basic_bloom_filter<_T, _Size, _HashFunctions>
    operator|(const basic_bloom_filter<_T, _Size, _HashFunctions>& lhs,
	      const basic_bloom_filter<_T, _Size, _HashFunctions>& rhs)
    {
      basic_bloom_filter<_T, _Size, _HashFunctions> ret(lhs);
      ret |= rhs;
      return ret;
    }

    template<class _T, size_t _Size, class _HashFunctions>
    basic_bloom_filter<_T, _Size, _HashFunctions>
    operator&(const basic_bloom_filter<_T, _Size, _HashFunctions>& lhs,
	      const basic_bloom_filter<_T, _Size, _HashFunctions>& rhs)
    {
      basic_bloom_filter<_T, _Size, _HashFunctions> ret(lhs);
      ret &= rhs;
      return ret;
    }

    template<class _T, size_t _Size, class _HashFunctions>
    void
    swap(basic_bloom_filter<_T, _Size, _HashFunctions>& lhs,
	 basic_bloom_filter<_T, _Size, _HashFunctions>& rhs)
    {
      lhs.swap(rhs);
    }
  } // namespace bloom_filters
} // namespace boost
#endif


//#define BZ_DEBUG

#ifdef BZ_DEBUG
#warning BZ_DEBUG aktiviert!
#define DEBUG_MSG "BZ_DEBUG aktiviert! "
#else
#warning ohne BZ_DEBUG
#define DEBUG_MSG "ohne BZ_DEBUG: "
#endif

#include <cstdio>
#include <cmath>
#include <string>
#include <iostream>
#include <sstream>
#include <fstream>
#include <typeinfo>
#include <stdexcept>
#include <complex>
#include <algorithm>
#include <Magick++.h>
#include <fftw3.h>
#include <sys/time.h>
#include <boost/any.hpp>
#include <boost/program_options.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/numeric/conversion/bounds.hpp>
#include <boost/limits.hpp>
#include <blitz/array.h>
#include "fm3.h"
#include "sourcecode.h"

 template <class T>
 inline std::string stringify (const T& t)
 {
	  std::stringstream ss;
	  ss << t;
	  return ss.str();
 }

using namespace std;
using namespace Magick;
using namespace blitz;
using namespace boost::program_options;
using namespace boost::algorithm;

template<typename T> inline T sqr(T x) { return x*x; };
template<typename T> inline T identity(T x) { return x; }
inline double deg2rad(double x) { return x*M_PI/180.0; };
inline double rad2deg(double x) { return x*180.0/M_PI; };

double y_mxPlusB(double x, double x1, double y1, double x2, double y2) {
	double m=(y2-y1)/(x2-x1);
	double b=y1-m*x1;
	return m*x+b;
}

template<typename T1, typename T2> void checkBound(Array<T1, 2> &a, Array<T2, 2> &b) {
	for(int i=0; i<2; i++) {
		FIF(a.lbound(i)!=b.lbound(i));
		FIF(a.ubound(i)!=b.ubound(i));
	}
};

void imageCacheToArray(unsigned char *ic, Array<int, 2> &a) {
	for(int y=a.ubound(1); y>=a.lbound(1); y--) {
		for(int x=a.lbound(0); x<=a.ubound(0); x++) {
			a(x, y)=*(ic++);
		}
	}
}

void plotAsciiPixel(double v) {
	int x=int(v*10.999);
	if(x>10) { printf("^"); return; }
	if(x<0) { printf("v"); return; }
	switch(x) {
		case 10: printf(" "); break;
		case 9: printf("·"); break;
		case 8: printf(":"); break;
		case 7: printf("¬"); break;
		case 6: printf("÷"); break;
		case 5: printf("Y"); break;
		case 4: printf("I"); break;
		case 3: printf("U"); break;
		case 2: printf("D"); break;
		case 1: printf("N"); break;
		case 0: printf("Ø"); break;
	}
}

void printArrayAscii(Array<int, 2> &a) {
	printf("%d\n", a.ubound(1)-a.lbound(1));
	for(int y=a.ubound(1); y>=a.lbound(1)+1; y-=2) {
		printf("%5d:", y);
		for(int x=a.lbound(0); x<=a.ubound(0); x++) {
			plotAsciiPixel((a(x, y)+a(x, y-1))/2.0/255.0);
		}
		printf(" :%d\n", y);
	}
}

inline int intImageCenterMax(int x) { return (x-1)/2; }
inline int intImageCenterMin(int x) { return intImageCenterMax(x)-(x-1); }

void derivateTo(
		int (*diffTransform)(int),
		Array<int, 2> &from,
		Array<int, 2> &deri,
		double rxD, double ryD) {

	checkBound(from, deri);

	int rx=int(round(rxD));
	int ry=int(round(ryD));
	for(int x=from.lbound(0); x<=from.ubound(0); x++) {
		for(int y=from.lbound(1); y<=from.ubound(1); y++) {
			Array<int, 2> window(from,
				Range(max(from.lbound(0), x-rx),
					min(from.ubound(0), x+rx)),
				Range(max(from.lbound(1), y-ry),
					min(from.ubound(1), y+ry))
					); // Achtung: Konstruktor -> Referenz, NICHT Kopie!
			int m=int(round(mean(window)));
			deri(x, y)=diffTransform(from(x, y)-m);
		}
	}
}

class dRange: public Range {
public:
	int iMin, iMax;
	double dMin, dMax;

	dRange(int iMin, int iMax, double dMin, double dMax):
		Range(iMin, iMax),
		iMin(iMin), iMax(iMax),
		dMin(dMin), dMax(dMax) {}

	double indexToReal(double x) {
		return y_mxPlusB(x, iMin, dMin, iMax, dMax);
		//return (x-iMin)/(iMax-iMin)*(dMax-dMin)+dMin;
	}

	int realToIndex(double x) {
		return int(round(
				y_mxPlusB(x, dMin, iMin, dMax, iMax)
				));
		/*return int(round(
				(x-dMin)/(dMax-dMin)*(iMax-iMin)+iMin
				));*/
	}
};

int theoreticMaxDForImage(Array<int, 2> &a) {
	return int(ceil(sqrt(
			sqr(a.ubound(0))+sqr(a.ubound(1))
			)));
}

int theoreticMinDForImage(Array<int, 2> &a) {
	return int(floor(-sqrt(
			sqr(a.lbound(0))+sqr(a.lbound(1))
			)));
}

inline double sin90(double x) {	return sin(x+M_PI/2); }
inline double cos90(double x) { return cos(x+M_PI/2); }

void imageToDrt(Array<int, 2> &img, Array<int, 2> &drt, dRange &thetaRange) {
	for(int thetaIndex=drt.lbound(0); thetaIndex<=drt.ubound(0); thetaIndex++) {
		double theta=thetaRange.indexToReal(thetaIndex);

		Array<int, 1> summandYA(Range(img.lbound(1), img.ubound(1)));
		for(int y=img.lbound(1); y<=img.ubound(1); y++)
			summandYA(y)=int(round(65536.0*(sin90(theta)*y +0.5)));

		for(int x=img.lbound(0); x<=img.ubound(0); x++) {
			int summandX=int(round(65536.0*cos90(theta)*x));
			for(int y=img.lbound(1); y<=img.ubound(1); y++) {
				drt(thetaIndex, (summandYA(y)+summandX)>>16)
					+=img(x, y);
			}
		}

	}
}

void normalize(Array<int, 2> &a, int toMaxVal) {
	int aMin=min(a);
	int aMax=max(a);
	a=((a-aMin)*toMaxVal+(aMax-aMin)/2)/(aMax-aMin);
}

void normalize(Array<double, 2> &a) {
	double aMin=min(a);
	double aMax=max(a);
	a=(a-aMin)/(aMax-aMin);
}

void buildAxis(Array<int, 2> &drt, int t1, int d1, int t2, int d2, Array<int, 1> sharpDrtAxis) {
	double tStep=double(t2-t1)/(d2-d1);
	double t=t1+0.5;
	for(int d=d1; d<=d2; d++) {
		int t_int=int(floor(t));
		if(t_int<drt.lbound(firstDim)) t_int=drt.lbound(firstDim);
		if(t_int>drt.ubound(firstDim)) t_int=drt.ubound(firstDim);
		sharpDrtAxis(d)=drt(t_int, d);
		t+=tStep;
	}
}

void findMaxDrtAxis(Array<int, 2> &drt, dRange &thetaRange, double tStart, double tStop, Array<int, 2> &img, int *pT1, int *pT2, int *pT1GleichT2) {
	int indexTStart=thetaRange.realToIndex(tStart);
	int indexTStop =thetaRange.realToIndex(tStop);
	//!*!*!*!*!*!*! double Max=std::numeric_limits<double>::min(); // min()>0!!!
	double Max=boost::numeric::bounds<double>::lowest(); // -1E308
	double t1GleichT2Max=Max;
	for(int t1=indexTStart; t1<=indexTStop; t1++) {
		int d1=img.lbound(1);
		for(int t2=indexTStart; t2<=indexTStop; t2++) {
			int d2=img.ubound(1);
			Array<int, 1> sharpDrtAxis(Range(d1, d2));
			buildAxis(drt, t1, d1, t2, d2, sharpDrtAxis);
			double m=mean(sharpDrtAxis);
			if(m>=Max) { // *pT1+*pT2 werden sonst ggf. nie gesetzt - z. B. bei ganz weißem Bild.
				Max=m;
				*pT1=t1;
				*pT2=t2;
			}
			if(t1==t2 && m>=t1GleichT2Max) { // dito
				t1GleichT2Max=m;
				*pT1GleichT2=t1;
			}
		}
	}
}

void writeToImage(Array<double, 2> &a, Image &i) {
	for(int y=a.ubound(1), yImage=0; y>=a.lbound(1); y--, yImage++) {
		for(int x=a.lbound(0), xImage=0; x<=a.ubound(0); x++, xImage++) {
			if(a(x, y)<0) i.pixelColor(xImage, yImage, "blue");
			else
				if(a(x, y)>1) i.pixelColor(xImage, yImage, "red");
				else i.pixelColor(xImage, yImage, Magick::ColorGray(a(x, y)));
		}
	}
}

double calcSchwerpunktD(Array<int, 1> &sharpDrtAxis) {
	double sumD=0;
	double n=0;
	for(int d=sharpDrtAxis.lbound(0); d<=sharpDrtAxis.ubound(0); d++) {
		sumD+=d*sharpDrtAxis(d);
		n+=sharpDrtAxis(d);
	}
	return sumD/n;
}

class fftPoint {
public:
	double abs;
	double halfWavelength; /* nur historische Gründe */
	double lines;
	fftPoint() {}
	fftPoint(double a, double b, double c): abs(a), halfWavelength(b), lines(c) {}

};
bool operator<(const fftPoint& a, const fftPoint& b) {
    return a.abs < b.abs;
}

double sd(double sumX, double sumXX, double length) {
	if(length==0.0) { return 0.0; }
	return sqrt((sumXX-sqr(sumX)/length)/length);
}

class drtParams {
public:
	double zoomFactor;
	double graphW2;
	double graphH2;
	double graphW;
	double radT1;
	double radT2;
	int drtAxisFftLength;
	vector<fftPoint> drtAxisFftOrderedByAbsDesc;
	vector<double> drtAxis;
};

void getDrtParamsFromImage(variables_map &cmdLine, Image &graphImg, drtParams &drtParam) {
	double pmMaxTheta=deg2rad(cmdLine["angle"].as<double>());
	double thetaCompare=deg2rad(cmdLine["compareAngle"].as<double>());

	graphImg.type( GrayscaleType );

	graphImg.normalize();
	graphImg.negate(true);

	if (cmdLine.count("zoom")) {
		cout << "zooming image to " << cmdLine["zoom"].as<string>() << "\n";
		graphImg.scale(cmdLine["zoom"].as<string>());
	}
	drtParam.zoomFactor=graphImg.size().height()/(drtParam.graphH2*2);

	unsigned char *imageCache=new unsigned char[graphImg.size().width()*graphImg.size().height()];
	graphImg.write(0, 0, graphImg.size().width(), graphImg.size().height(), "R", CharPixel, imageCache);

	Range imageRangeX(intImageCenterMin(graphImg.size().width() ), intImageCenterMax(graphImg.size().width() ));
	Range imageRangeY(intImageCenterMin(graphImg.size().height()), intImageCenterMax(graphImg.size().height()));

	Array<int, 2> img(imageRangeX, imageRangeY);
	imageCacheToArray(imageCache, img);
	delete imageCache;

	Array<int, 2> sharpImg(imageRangeX, imageRangeY);

	printf("sharpening image..."); fflush(stdout);
	derivateTo(identity, img, sharpImg,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100);
	printf("done.\n");

	double maxWinkelRad=pmMaxTheta +thetaCompare;
	dRange drtThetaRange(
			int(round(-maxWinkelRad*graphImg.size().height())),
			int(round( maxWinkelRad*graphImg.size().height())),
			-maxWinkelRad,
			 maxWinkelRad
			);

	dRange drtDRange(theoreticMinDForImage(sharpImg), theoreticMaxDForImage(sharpImg),
			-double(theoreticMinDForImage(sharpImg))/sharpImg.lbound(1)/2,
			double(theoreticMaxDForImage(sharpImg))/sharpImg.ubound(1)/2);

	Array<int, 2> drt(drtThetaRange, drtDRange);
	drt=0; // Initialisierung

	printf("imageToDrt..."); fflush(stdout);
	imageToDrt(sharpImg, drt, drtThetaRange);
	printf("done.\n");

	Array<int, 2> raiseDrt(drtThetaRange, drtDRange);
	printf("raising drt..."); fflush(stdout);
	derivateTo(abs, drt, raiseDrt,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100);

	printf("sharpening drt..."); fflush(stdout);
	Array<int, 2> sharpDrt(drtThetaRange, drtDRange);
	derivateTo(identity, drt, sharpDrt,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100,
			graphImg.size().height()*cmdLine["sr"].as<double>()/100);
	printf("done.\n");

	int t1, t2, t1GleichT2;
	int d1=sharpImg.lbound(1);
	int d2=sharpImg.ubound(1);
	printf("find max(raiseDrt)..."); fflush(stdout);

	findMaxDrtAxis(raiseDrt, drtThetaRange,
		-pmMaxTheta,
		 pmMaxTheta,
		sharpImg, &t1, &t2, &t1GleichT2);
	printf("done.\n");

	drtParam.radT1=drtThetaRange.indexToReal(t1);
	drtParam.radT2=drtThetaRange.indexToReal(t2);

	printf("angle at 0%%: %f; angle at 100%%: %f degrees\n", rad2deg(drtParam.radT1), rad2deg(drtParam.radT2));
	printf("max. var. at constant angle: %lf degrees\n", rad2deg(drtThetaRange.indexToReal(t1GleichT2)));

	//

	Array<int, 1> sharpDrtAxis(Range(d1, d2));
	buildAxis(sharpDrt, t1, d1, t2, d2, sharpDrtAxis);

	double doubleDrtAxis[d2-d1+1];
	printf("iDrtAxisVal:");
	for(int d=d1; d<=d2; d++) {
		doubleDrtAxis[d-d1]=double(sharpDrtAxis(d))/(sharpImg.ubound(0)-sharpImg.lbound(0)+1);
		printf(" %f", doubleDrtAxis[d-d1]);
		drtParam.drtAxis.push_back(doubleDrtAxis[d-d1]);
	}
	printf("\n");

	fftw_plan fftPlan;
	drtParam.drtAxisFftLength=(d2-d1+1)/2+1;
	complex<double>* fft = new complex<double>[drtParam.drtAxisFftLength];
	drtParam.drtAxisFftOrderedByAbsDesc=vector<fftPoint>(drtParam.drtAxisFftLength-1);
	//vector<fftPoint> drtParam.drtParam.drtAxisFftOrderedByAbsDesc(drtAxisFftLength-1);
/*  C++ has its own complex<T> template class, defined in the standard <complex> header file.
	  Reportedly, the C++ standards committee has recently agreed to mandate that the storage
	format used for this type be binary-compatible with the C99 type, i.e. an array T[2] with consecutive
	real [0] and imaginary [1] parts. (See report WG21/N1388.)
	  Although not part of the official standard as of this writing, the proposal stated that:
	“This solution has been tested with all current major implementations of the standard library and
	shown to be working.”
	  To the extent that this is true, if you have a variable complex<double> *x, you can pass it directly
	to FFTW via reinterpret_cast<fftw_complex*>(x).*/
	fftPlan = fftw_plan_dft_r2c_1d(d2-d1+1, doubleDrtAxis, reinterpret_cast<fftw_complex*>(fft), FFTW_ESTIMATE);
	fftw_execute(fftPlan);
	printf("iDrtAxisFFT(n=%d):", d2-d1+1);
	for(int i=1; i<drtParam.drtAxisFftLength; i++) {
		drtParam.drtAxisFftOrderedByAbsDesc[i-1].abs=abs(fft[i])/drtParam.drtAxisFftLength;
		// nächste Zeile: 33% (=0.33) war der default-Zoomfaktor der alten drt-Version.
		// damit die Zahlen vergleichbar sind, wird umgerechnet.
		drtParam.drtAxisFftOrderedByAbsDesc[i-1].halfWavelength=double(drtParam.drtAxisFftLength)*0.33/drtParam.zoomFactor/i;
		drtParam.drtAxisFftOrderedByAbsDesc[i-1].lines=i;
		printf(" %f", drtParam.drtAxisFftOrderedByAbsDesc[i-1].abs);
	}
	printf("\n");
	fftw_destroy_plan(fftPlan);
	delete[] fft;

	//

	Array<int, 1> raiseDrtAxis(Range(d1, d2));
	buildAxis(raiseDrt, t1, d1, t2, d2, raiseDrtAxis);
	double dSchwerRel=(calcSchwerpunktD(raiseDrtAxis)-d1)/(d2-d1);

	Array<int, 1> raiseDrtAxisVar1(Range(d1, d2));
	buildAxis(raiseDrt,
		t1-drtThetaRange.realToIndex(thetaCompare*   dSchwerRel ), d1,
		t2+drtThetaRange.realToIndex(thetaCompare*(1-dSchwerRel)), d2, raiseDrtAxisVar1);

	Array<int, 1> raiseDrtAxisVar2(Range(d1, d2));
	buildAxis(raiseDrt,
		t1+drtThetaRange.realToIndex(thetaCompare*   dSchwerRel ), d1,
		t2-drtThetaRange.realToIndex(thetaCompare*(1-dSchwerRel)), d2, raiseDrtAxisVar2);

	double var1qual=mean(raiseDrtAxisVar1)/mean(raiseDrtAxis);
	double var2qual=mean(raiseDrtAxisVar2)/mean(raiseDrtAxis);
	printf("drtAxis Var1:%f%% Var2:%f%% => %f\n",
			var1qual*100, var2qual*100, 100*(1-max(var1qual, var2qual)));

	if(cmdLine.count("drt")) {
		string sizeStr=stringify(drtThetaRange.iMax-drtThetaRange.iMin+1)
							+"x"+stringify(drtDRange.iMax-drtDRange.iMin+1);
		Image drtOut(sizeStr.c_str(), "black");
		drtOut.quality(100);

		Array<double, 2> tmpDrt(drtThetaRange, drtDRange);
		tmpDrt=raiseDrt*1.0;
		normalize(tmpDrt);

		double tStep=double(t2-t1)/(d2-d1);
		double t=t1+0.5;
		for(int d=d1; d<=d2; d++) {
			tmpDrt(int(floor(t)), d)=-1.0;
			t+=tStep;
		}

		writeToImage(tmpDrt, drtOut);

		try {
			drtOut.write(cmdLine["drt"].as<string>());
		}
		catch( Magick::WarningCoder &warning ) {
			cerr << "Coder Warning: " << warning.what() << endl;
		}
	    catch( Magick::Warning &warning ) {
	    	cerr << "Warning: " << warning.what() << endl;
	    }
	    catch( Magick::ErrorBlob &error) {
	    	cerr << "Error: " << error.what() << endl;
	    }
	}
}

string &after(string &haystack, const char *key, string &val) {
	if(haystack.find(key, 0)!=0) {
	    val=string("");
	    return val;
	}
	val=haystack.substr(strlen(key));
	return val;
}

int getDrtParamsFromDrtlog(variables_map &cmdLine, Image &graphImg, drtParams &drtParam) {
	drtParam.zoomFactor=0.33;

	ifstream drtLog;
	drtLog.open(cmdLine["import"].as<string>().c_str());
	if(drtLog.fail()) return 0;

	int paramcount=0;
	while(! drtLog.eof()) {
		string line;
		getline(drtLog, line);
		string val;

//			graphW/H wird nicht mehr angefasst, da bereits beim Laden des Bildes bestimmt.

		if((after(line, "size: ", val)).size()>0) {
			vector<string> size;
			split(size, val, is_any_of("x"));
			if(strtoul(size[0].c_str(), NULL, 10) != graphImg.size().width()
			|| strtoul(size[1].c_str(), NULL, 10) != graphImg.size().height()) {
				printf("FEHLER: Bildgröße aus %s stimmt nicht mit tatsächlicher Bildgröße überein.\n", cmdLine["import"].as<string>().c_str());
				return 0;
			}
		}

		if((after(line, "angle at ", val)).size()>0) {
			vector<string> angleParam;
			split(angleParam, val, is_any_of("%:; ")); // trennzeichen zusammenfassen

			double perD1=atof(angleParam[0].c_str());
			double radT1=deg2rad(atof(angleParam[3].c_str()));
			double perD2=atof(angleParam[7].c_str());
			double radT2=deg2rad(atof(angleParam[10].c_str()));

			drtParam.radT1=y_mxPlusB(  0, perD1, radT1, perD2, radT2);
			drtParam.radT2=y_mxPlusB(100, perD1, radT1, perD2, radT2);
			printf("radT1=%f\n", drtParam.radT1);
			printf("radT2=%f\n", drtParam.radT2);
			paramcount++;
		}

		if((after(line, "iDrtAxisFFT(n=", val)).size()>0) {
			vector<string> sfft;
			split(sfft, val, is_any_of(" "));
			vector<string> n;
			split(n, sfft[0], is_any_of(")"));
			int fftInputLength=atoi(n[0].c_str());
			drtParam.drtAxisFftLength=(fftInputLength-1)/2;
			// ja, war wohl in altem drt-Programm so;
			// die paar % machen den Kohl nicht fett.
			printf("drtParam.drtAxisFftLength=%d\n", drtParam.drtAxisFftLength);
			for(unsigned i=1; i<sfft.size(); i++) {
				drtParam.drtAxisFftOrderedByAbsDesc.push_back(fftPoint(
						atof(sfft[i].c_str()),
						double(drtParam.drtAxisFftLength)/i,
						double(i)
						));
			}
			paramcount++;
		}

		if((after(line, "iDrtAxisVal: ", val)).size()>0) {
			vector<string> string_drtAxis;
			split(string_drtAxis, val, is_any_of(" "));
			for(unsigned i=0; i<string_drtAxis.size(); i++) {
				drtParam.drtAxis.push_back(atof(string_drtAxis[i].c_str()));
			}
			paramcount++;
		}

		// "shrinking"="zooming"
		if((after(line, "shrinking image to ", val)).size()>0) {
			vector<string> zoom;
			split(zoom, val, is_any_of("%"));
			drtParam.zoomFactor=atof(zoom[0].c_str())/100;
			printf("zoomFactor=%f\n", drtParam.zoomFactor);
			paramcount++;
		}
		if((after(line, "zooming image to ", val)).size()>0) {
			vector<string> zoom;
			split(zoom, val, is_any_of("%"));
			drtParam.zoomFactor=atof(zoom[0].c_str())/100;
			printf("zoomFactor=%f\n", drtParam.zoomFactor);
			paramcount++;
		}
	}

	if(paramcount!=4) return 0;
	return 1;
}

int main(int argc, char **argv)
{
	FIF(sizeof(int)!=4);
	FIF((3>>1)!=1);
	FIF((-3>>1)!=-2);

	options_description optDesc(string(DEBUG_MSG)+string(__FILE__)+" @ "+string(__DATE__)+" "+string(__TIME__)+"\nAllowed options");
	optDesc.add_options()
	    ("help", "produce help message")
	    ("source", "print program source code")
	    ("if",  value<string>(), "input image file")
	    ("import", value<string>(), "import drtLog file")
	    ("drt", value<string>(), "drt image file (visualization of drt)")
	    ("tmpfile",  value<string>(), "temporary image file (format conversion only); will not be deleted")
	    ("of",  value<string>(), "output image file (after spherical correction)")
	    ("python", value<string>()->default_value("python"), "python command (requires PIL)")
	    ("doTrans", "does format conversion with transCmd, if applicable (sd<maxSd)")
	    ("noTransDeskew", "format conversion&statistics only")
	    ("maxSd", value<double>()->default_value(0.8), "max. sd(halfWavelength) allowing transformation")
	    ("sr",  value<double>()->default_value(1.0), "shapen radius [image height %]")
	    ("zoom",  value<string>()->default_value("33%"), "zoom +'%'")
	    ("angle",  value<double>()->default_value(5.0), "drt angle probe range [Grad]")
	    ("compareAngle",  value<double>()->default_value(1.0), "+/- angle variation to compare")
	    ("halfWavelengthVarianceAbsFactor",  value<double>()->default_value(50), "variance(halfWavelength(fft where abs(fft)>=max(abs(fft))*hWVAF))")

	    ("minPageHeight",  value<double>()->default_value(150.0), "page height minus border[mm]")
	    ("maxPageHeight",  value<double>()->default_value(600.0), "page height minus border[mm]")
	    ("minLineSpaceSize", value<double>()->default_value(10.0), "[pt] (1/72 inch = 0.35277mm)")
	    ("maxLineSpaceSize", value<double>()->default_value(16.0), "[pt] (1/72 inch = 0.35277mm)")
	    ("minLineSpaceRatio", value<double>()->default_value(0.7))
		("maxLineSpaceRatio", value<double>()->default_value(2.0))
		("minLineLike", value<double>()->default_value(26.664827), "%")
		("hysteresis", value<double>()->default_value(4.278268))
		("maxSdLineSpace", value<double>()->default_value(6.272757))
		("gamma", value<double>()->default_value(2.2), "sRGB gamma correction before(1/x)&after(x) transform")
	;

	variables_map cmdLine;
	store(parse_command_line(argc, argv, optDesc), cmdLine);
	notify(cmdLine);

	if (cmdLine.count("help")) {
	    cout << optDesc << "\n";
	    return 1;
	}
	if (cmdLine.count("source")) {
		cerr << SOURCECODE_TIMESTAMP << endl;
	    fwrite(SOURCECODE, 1, SOURCECODE_LENGTH, stdout);
	    return 1;
	}

	FIF(!cmdLine.count("if"));

	Image graphImg;
	printf("reading image..."); fflush(stdout);
	try {
		graphImg.read(cmdLine["if"].as<string>()); printf("done.\n");
	}
	catch( Magick::WarningCoder &warning ) {
		cerr << "Coder Warning: " << warning.what() << endl;
	}
	catch( Magick::Warning &warning ) {
		cerr << "Warning: " << warning.what() << endl;
	}
	catch( Magick::ErrorBlob &error) {
		cerr << "Error: " << error.what() << endl;
	}

	Image transImg=graphImg; // wird unten gebraucht

	graphImg.modifyImage();
	//SetImageVirtualPixelMethod( graphImg.image(), MagickCore::WhiteVirtualPixelMethod);
	//TODO: will in aktueller Version noch nicht
	// graphImg.options()->virtualPixelMethod( MagickCore::WhiteVirtualPixelMethod );

	graphImg.virtualPixelMethod(MagickCore::WhiteVirtualPixelMethod); // aber auch das will in Magick++ unter (ubuntu) karmic koala nicht

	drtParams drtParam;
	printf("size: %lux%lu\n", graphImg.size().width(), graphImg.size().height());
	drtParam.graphW2=double(graphImg.size().width())/2;
	drtParam.graphH2=double(graphImg.size().height())/2;
	drtParam.graphW=drtParam.graphW2*2;

	if(cmdLine.count("tmpfile")) {
		// ist ja nur ein tmpfile: graphImg.density(Geometry(300, 300)); // 300 dpi setzen
		graphImg.quality(100);
		printf("saving image..."); fflush(stdout);
		//graphImg.write(cmdLine["tmpfile"].as<string>());
		printf("done.\n");
	}

	if(!cmdLine.count("import")) {
		getDrtParamsFromImage(cmdLine, graphImg, drtParam);
	} else {
		if(!getDrtParamsFromDrtlog(cmdLine, graphImg, drtParam)) {
			fprintf(stderr, "WARNUNG: drtLog-(import)-Datei %s fehlerhaft. DRT-Analyse wird vom Bild durchgefuehrt.\n", cmdLine["import"].as<string>().c_str());
			getDrtParamsFromImage(cmdLine, graphImg, drtParam);
		}
	}

	double radT12avg=(drtParam.radT1+drtParam.radT2)/2;
	double radT1rel=drtParam.radT1-radT12avg;
	double radT2rel=drtParam.radT2-radT12avg;

	// 0 3
	// 1 2

	vector<complex<double> > transKoord(4);

	if(radT2rel<0) {
		transKoord[0]=complex<double>(-drtParam.graphW2, drtParam.graphH2);
		transKoord[3]=transKoord[0]+polar(drtParam.graphW, radT2rel);
	} else {
		transKoord[3]=complex<double>(drtParam.graphW2, drtParam.graphH2);
		transKoord[0]=transKoord[3]-polar(drtParam.graphW, radT2rel);
	}

	if(radT1rel>0) {
		transKoord[1]=complex<double>(-drtParam.graphW2, -drtParam.graphH2);
		transKoord[2]=transKoord[1]+polar(drtParam.graphW, radT1rel);
	} else {
		transKoord[2]=complex<double>(drtParam.graphW2, -drtParam.graphH2);
		transKoord[1]=transKoord[2]-polar(drtParam.graphW, radT1rel);
	}

	double distortParam[16];
	for(int i=0; i<4; i++) {
		int x, y;
		switch(i) {
			case 0: x=0; y=0; break;
			case 1: x=0; y=drtParam.graphH2*2-1; break;
			case 2: x=drtParam.graphW2*2-1; y=drtParam.graphH2*2-1; break;
			case 3: x=drtParam.graphW2*2-1; y=0; break;
		}
		double r=abs(transKoord[i]);
		double phi=std::arg(transKoord[i])+radT12avg;
		distortParam[i*4+0]=r*cos(phi)+drtParam.graphW2; //+pilXoff;
		distortParam[i*4+1]=(drtParam.graphH2*2-1)-(r*sin(phi)+drtParam.graphH2); //+pilYoff;
		distortParam[i*4+2]=x;
		distortParam[i*4+3]=y;

		printf("%d: %f,%f %f,%f\n", i, distortParam[i*4+0], distortParam[i*4+1], distortParam[i*4+2], distortParam[i*4+3]);
	}

	sort(drtParam.drtAxisFftOrderedByAbsDesc.begin(), drtParam.drtAxisFftOrderedByAbsDesc.end());
	reverse(drtParam.drtAxisFftOrderedByAbsDesc.begin(), drtParam.drtAxisFftOrderedByAbsDesc.end());
	printf("iDrtAxisFFT ordered by abs: ");
	double sumW=0;
	double sumL=0;
	double sumWW=0;
	int n=0;
	for(int i=0; i<drtParam.drtAxisFftLength-1
		&& drtParam.drtAxisFftOrderedByAbsDesc[i].abs>=
			drtParam.drtAxisFftOrderedByAbsDesc[0].abs
				*cmdLine["halfWavelengthVarianceAbsFactor"].as<double>()/100;
		i++) {
		sumW+=drtParam.drtAxisFftOrderedByAbsDesc[i].halfWavelength;
		sumWW+=pow(drtParam.drtAxisFftOrderedByAbsDesc[i].halfWavelength, 2);
		sumL+=drtParam.drtAxisFftOrderedByAbsDesc[i].lines;
		n++;
		printf(" (%.3lf:%.3lf)", drtParam.drtAxisFftOrderedByAbsDesc[i].abs,
				drtParam.drtAxisFftOrderedByAbsDesc[i].halfWavelength);
	}
	printf("\n");
	printf("sd=%lf halfWl=%lf lines=%lf\n", sd(sumW, sumWW, n), sumW/n, sumL/n);

	// neue Evaluierung der Drt-Achse:

	double minLines=cmdLine["minPageHeight"].as<double>() / (cmdLine["maxLineSpaceSize"].as<double>()*0.35277);
	double maxLines=cmdLine["maxPageHeight"].as<double>() / (cmdLine["minLineSpaceSize"].as<double>()*0.35277);
	int minLineSpace=int(drtParam.drtAxis.size()/maxLines);
	int maxLineSpace=int(drtParam.drtAxis.size()/minLines+0.999);
	printf("minLineSpace:%d\n", minLineSpace);
	printf("maxLineSpace:%d\n", maxLineSpace);
	double minRatio=cmdLine["minLineSpaceRatio"].as<double>();
	double maxRatio=cmdLine["maxLineSpaceRatio"].as<double>();

	double line=0;
	double space=0;
	double sumLine=0;
	double sumLine2=0;
	double sumSpace=0;
	double sumSpace2=0;
	double sumN=0;

	int state=0;
	double hyst=cmdLine["hysteresis"].as<double>();

	double insgesamt=0;
	double zeilenartig=0;

	if(drtParam.drtAxis[0]>0) { state=1; space++; } else { state=0; line++; }

	printf("iDrtAxis-LineSpacePattern:");
	for(unsigned i=1; i<drtParam.drtAxis.size(); i++) {
		if(state==0) {
			if(drtParam.drtAxis[i]>hyst) {
				printf("(l:%.0lf;s:%.0lf)s", line, space);
				if(space==0.0) { space+=0.000001; }
				double ratio=line/space;
				insgesamt+=line+space;
				if(line+space>=minLineSpace && line+space<=maxLineSpace)	{
					if(ratio>=minRatio/1.55 && ratio<=maxRatio*1.55) {
						sumLine+=line;
						sumLine2+=line*line;
						sumSpace+=space;
						sumSpace2+=space*space;
						sumN++;
					}
					if(ratio>=minRatio && ratio<=maxRatio) {
						zeilenartig+=line+space;
					}
				}
				line=0; space=1;
				state=1;
			} else { printf("l"); line++; } // hyst. n. überschr.
		} else { // status=1
			if(drtParam.drtAxis[i]<(-hyst)) {
				line++;
				printf("l");
				state=0;
			} else { printf("s"); space++; }
		}
	}
	printf("\n");
	double sdLine=sd(sumLine, sumLine2, sumN);
	double sdSpace=sd(sumSpace, sumSpace2, sumN);
	double sdLineSpace=sqrt(sdLine*sdLine+sdSpace*sdSpace);

	double proz_zeilenartig=double(zeilenartig)*100.0/(insgesamt+boost::numeric::bounds<double>::smallest()); // sicherheitshalber, falls weißes Blatt etc.
	printf("line-like: %.2f%%; sdLineSpace=%.3f\n", proz_zeilenartig, sdLineSpace);

	if(cmdLine.count("doTrans") && cmdLine.count("of")) {
		//if(sd(sumW, sumWW, n) < cmdLine["maxSd"].as<double>() && cmdLine.count("noTransDeskew")==0)
		// wird nun in doUnzip...sh erledigt: transImg.density(Geometry(300, 300)); // 300 dpi setzen
		if(proz_zeilenartig >= cmdLine["minLineLike"].as<double>()
				&& sdLineSpace <= cmdLine["maxSdLineSpace"].as<double>()
				&& cmdLine.count("noTransDeskew")==0) {
			transImg.compressType(NoCompression);
			transImg.depth(16);
			transImg.gamma(1/cmdLine["gamma"].as<double>());
			transImg.distort(MagickCore::PerspectiveDistortion,  16, distortParam);
			transImg.gamma(cmdLine["gamma"].as<double>());
			transImg.depth(8);
		}
		try {
			transImg.write(cmdLine["of"].as<string>());
		}
		catch( Magick::WarningCoder &warning ) {
			cerr << "Coder Warning: " << warning.what() << endl;
		}
		catch( Magick::Warning &warning ) {
			cerr << "Warning: " << warning.what() << endl;
		}
		catch( Magick::ErrorBlob &error) {
			cerr << "Error: " << error.what() << endl;
		}
	}
}


/**
 * Copyright (c) 2011-2017 libbitcoin developers (see AUTHORS)
 *
 * This file is part of libbitcoin.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Affero General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
#include <bitcoin/bitcoin/formats/base_58.hpp>

#include <boost/algorithm/string.hpp>
#include <bitcoin/bitcoin/utility/assert.hpp>

namespace libbitcoin {

const std::string base58_chars =
    "123456789ABCDEFGHJKLMNPQRSTUVWXYZabcdefghijkmnopqrstuvwxyz";

bool is_base58(const char ch)
{
    // This works because the base58 characters happen to be in sorted order
    return std::binary_search(base58_chars.begin(), base58_chars.end(), ch);
}

bool is_base58(const std::string& text)
{
    const auto test = [](const char ch)
    {
        return is_base58(ch);
    };

    return std::all_of(text.begin(), text.end(), test);
}

template <typename Data>
auto search_first_nonzero(const Data& data) -> decltype(data.cbegin())
{
    auto first_nonzero = data.cbegin();
    while (first_nonzero != data.end() && *first_nonzero == 0)
        ++first_nonzero;

    return first_nonzero;
}

size_t count_leading_zeros(data_slice unencoded)
{
    // Skip and count leading '1's.
    size_t leading_zeros = 0;
    for (const uint8_t byte: unencoded)
    {
        if (byte != 0)
            break;

        ++leading_zeros;
    }

    return leading_zeros;
}

void pack_value(data_chunk& indexes, size_t carry)
{
    // Apply "b58 = b58 * 256 + ch".
    for (auto it = indexes.rbegin(); it != indexes.rend(); ++it)
    {
        carry += 256 * (*it);
        *it = carry % 58;
        carry /= 58;
    }

    BITCOIN_ASSERT(carry == 0);
}

std::string encode_base58(data_slice unencoded)
{
    size_t leading_zeros = count_leading_zeros(unencoded);

    // size = log(256) / log(58), rounded up.
    const size_t number_nonzero = unencoded.size() - leading_zeros;
    const size_t indexes_size = number_nonzero * 138 / 100 + 1;

    // Allocate enough space in big-endian base58 representation.
    data_chunk indexes(indexes_size);

    // Process the bytes.
    for (auto it = unencoded.begin() + leading_zeros;
        it != unencoded.end(); ++it)
    {
        pack_value(indexes, *it);
    }

    // Skip leading zeroes in base58 result.
    auto first_nonzero = search_first_nonzero(indexes);

    // Translate the result into a string.
    std::string encoded;
    const size_t estimated_size = leading_zeros +
        (indexes.end() - first_nonzero);
    encoded.reserve(estimated_size);
    encoded.assign(leading_zeros, '1');

    // Set actual main bytes.
    for (auto it = first_nonzero; it != indexes.end(); ++it)
    {
        const size_t index = *it;
        encoded += base58_chars[index];
    }

    return encoded;
}

size_t count_leading_zeros(const std::string& encoded)
{
    // Skip and count leading '1's.
    size_t leading_zeros = 0;
    for (const uint8_t digit: encoded)
    {
        if (digit != base58_chars[0])
            break;

        ++leading_zeros;
    }

    return leading_zeros;
}

void unpack_char(data_chunk& data, size_t carry)
{
    for (auto it = data.rbegin(); it != data.rend(); it++)
    {
        carry += 58 * (*it);
        *it = carry % 256;
        carry /= 256;
    }

    BITCOIN_ASSERT(carry == 0);
}

bool decode_base58(data_chunk& out, const std::string& in)
{
    // Trim spaces and newlines around the string.
    const auto leading_zeros = count_leading_zeros(in);

    // log(58) / log(256), rounded up.
    const size_t data_size = in.size() * 733 / 1000 + 1;

    // Allocate enough space in big-endian base256 representation.
    data_chunk data(data_size);

    // Process the characters.
    for (auto it = in.begin() + leading_zeros; it != in.end(); ++it)
    {
        const auto carry = base58_chars.find(*it);
        if (carry == std::string::npos)
            return false;

        unpack_char(data, carry);
    }

    // Skip leading zeroes in data.
    auto first_nonzero = search_first_nonzero(data);

    // Copy result into output vector.
    data_chunk decoded;
    const size_t estimated_size = leading_zeros + (data.end() - first_nonzero);
    decoded.reserve(estimated_size);
    decoded.assign(leading_zeros, 0x00);
    decoded.insert(decoded.end(), first_nonzero, data.cend());

    out = decoded;
    return true;
}

// For support of template implementation only, do not call directly.
bool decode_base58_private(uint8_t* out, size_t out_size, const char* in)
{
    data_chunk buffer;
    if (!decode_base58(buffer, in) || buffer.size() != out_size)
        return false;

    for (size_t i = 0; i < out_size; ++i)
        out[i] = buffer[i];

    return true;
}

} // namespace libbitcoin


/**
 * Copyright (c) 2018, University Osnabrück
 * 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 University Osnabrück 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 University Osnabrück 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.
 */

/**
 * EigenSVDPointAlign.hpp
 *
 *  @date Feb 21, 2014
 *  @author Thomas Wiemann
 */
#ifndef EIGENSVDPOINTALIGN_HPP_
#define EIGENSVDPOINTALIGN_HPP_

#include "SLAMScanWrapper.hpp"
#include "lvr2/types/MatrixTypes.hpp"

#include <Eigen/Dense>

namespace lvr2
{

template<typename T, typename PointT = float>
class EigenSVDPointAlign
{
public:
    using Vec3 = Vector3<T>;
    using Mat4 = Transform<T>;
    using Mat3 = Eigen::Matrix<T, 3, 3>;
    using Point3 = Vector3<PointT>;
    using PointPairVector = std::vector<std::pair<Point3, Point3>>;

    EigenSVDPointAlign() {};

    /**
     * @brief Calculates the estimated Transformation to match a Data Pointcloud to a Model
     *        Pointcloud
     *
     * Apply the resulting Transform to the Data Pointcloud.
     *
     * @param scan       The Data Pointcloud
     * @param neighbors  An array containing a Pointer to a neighbor in the Model Pointcloud for
     *                   each Point in `scan`, or nullptr if there is no neighbor for a Point
     * @param centroid_m The center of the Model Pointcloud
     * @param centroid_d The center of the Data Pointcloud
     * @param align      Will be set to the Transformation
     *
     * @return The average Point-to-Point error of the Scans
     */
    T alignPoints(
        SLAMScanPtr scan,
        Point3** neighbors,
        const Vec3& centroid_m,
        const Vec3& centroid_d,
        Mat4& align) const;

    /**
     * @brief Calculates the estimated Transformation to match a Data Pointcloud to a Model
     *        Pointcloud
     *
     * Apply the resulting Transform to the Data Pointcloud.
     *
     * @param points     A vector of pairs with (model, data) Points
     * @param centroid_m The center of the Model Pointcloud
     * @param centroid_d The center of the Data Pointcloud
     * @param align      Will be set to the Transformation
     *
     * @return The average Point-to-Point error of the Scans
     */
    T alignPoints(
        PointPairVector& points,
        const Vec3& centroid_m,
        const Vec3& centroid_d,
        Mat4& align) const;
};

} /* namespace lvr2 */

#include "EigenSVDPointAlign.tcc"

#endif /* EIGENSVDPOINTALIGN_HPP_ */


/**
 * Copyright (c) 2013 Jonan Cruz-Martin
 *
 * Distributed under the terms of the MIT license, see the accompanying
 * file COPYING or http://opensource.org/licenses/MIT.
 *
 * @file
 * @brief Implement calculation of the surface of a cell.
 */

#ifndef ALGORHITHMS_CELL_SURFACE_HPP
#define ALGORHITHMS_CELL_SURFACE_HPP

#include <boost/python.hpp>
using namespace boost::python;

#include "../points/cartesian.hpp"
#include "../points/cylindrical.hpp"
#include "../points/polar.hpp"
#include "../points/spherical.hpp"

#include "../cells/linear.hpp"
#include "../cells/triangular.hpp"
#include "../cells/quadrilateral.hpp"
#include "../cells/tetrahedral.hpp"

////////////
// Linear //
////////////

/**
 * Calculate the surface of a linear cartesian 1D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCartesian1D_Cell_surface(LinearCartesian1D_Cell cell);

/**
 * Calculate the surface of a linear cartesian 2D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCartesian2D_Cell_surface(LinearCartesian2D_Cell cell);

/**
 * Calculate the surface of a linear cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCartesian3D_Cell_surface(LinearCartesian3D_Cell cell);

/**
 * Calculate the surface of a linear cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearCylindrical3D_Cell_surface(LinearCylindrical3D_Cell cell);

/**
 * Calculate the surface of a linear polar cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearPolar2D_Cell_surface(LinearPolar2D_Cell cell);

/**
 * Calculate the surface of a linear spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double LinearSpherical3D_Cell_surface(LinearSpherical3D_Cell cell);

////////////////
// Triangular //
////////////////

/**
 * Calculate the surface of a triangular cartesian 2D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularCartesian2D_Cell_surface(TriangularCartesian2D_Cell cell);

/**
 * Calculate the surface of a triangular cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularCartesian3D_Cell_surface(TriangularCartesian3D_Cell cell);

/**
 * Calculate the surface of a triangular cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularCylindrical3D_Cell_surface(TriangularCylindrical3D_Cell cell);

/**
 * Calculate the surface of a triangular polar cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularPolar2D_Cell_surface(TriangularPolar2D_Cell cell);

/**
 * Calculate the surface of a triangular spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TriangularSpherical3D_Cell_surface(TriangularSpherical3D_Cell cell);

///////////////////
// Quadrilateral //
///////////////////

/**
 * Calculate the surface of a quadrilateral cartesian 2D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralCartesian2D_Cell_surface(QuadrilateralCartesian2D_Cell cell);

/**
 * Calculate the surface of a quadrilateral cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralCartesian3D_Cell_surface(QuadrilateralCartesian3D_Cell cell);

/**
 * Calculate the surface of a quadrilateral cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralCylindrical3D_Cell_surface(QuadrilateralCylindrical3D_Cell cell);

/**
 * Calculate the surface of a quadrilateral polar cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralPolar2D_Cell_surface(QuadrilateralPolar2D_Cell cell);

/**
 * Calculate the surface of a quadrilateral spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double QuadrilateralSpherical3D_Cell_surface(QuadrilateralSpherical3D_Cell cell);

/////////////////
// Tetrahedral //
/////////////////

/**
 * Calculate the surface of a tetrahedral cartesian 3D cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TetrahedralCartesian3D_Cell_surface(TetrahedralCartesian3D_Cell cell);

/**
 * Calculate the surface of a tetrahedral cylindrical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TetrahedralCylindrical3D_Cell_surface(TetrahedralCylindrical3D_Cell cell);

/**
 * Calculate the surface of a tetrahedral spherical cell.
 *
 * @param cell Cell whose surface will be computed.
 * @return Real number in double precision which represents the surface of the cell.
 *
 * @since 0.1.0
 */
double TetrahedralSpherical3D_Cell_surface(TetrahedralSpherical3D_Cell cell);

#endif /* end of include guard: ALGORHITHMS_CELL_SURFACE_HPP */


/* boost random/non_central_chi_squared_distribution.hpp header file
 *
 * Copyright Thijs van den Berg 2014
 *
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id$
 */

#ifndef BOOST_RANDOM_NON_CENTRAL_CHI_SQUARED_DISTRIBUTION_HPP
#define BOOST_RANDOM_NON_CENTRAL_CHI_SQUARED_DISTRIBUTION_HPP

#include <boost/config/no_tr1/cmath.hpp>
#include <iosfwd>
#include <istream>
#include <boost/limits.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/operators.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/poisson_distribution.hpp>

namespace boost {
namespace random {

/**
 * The noncentral chi-squared distribution is a real valued distribution with
 * two parameter, @c k and @c lambda.  The distribution produces values > 0.
 *
 * This is the distribution of the sum of squares of k Normal distributed
 * variates each with variance one and \f$\lambda\f$ the sum of squares of the
 * normal means.
 *
 * The distribution function is
 * \f$\displaystyle P(x) = \frac{1}{2} e^{-(x+\lambda)/2} \left( \frac{x}{\lambda} \right)^{k/4-1/2} I_{k/2-1}( \sqrt{\lambda x} )\f$.
 *  where  \f$\displaystyle I_\nu(z)\f$ is a modified Bessel function of the
 * first kind.
 *
 * The algorithm is taken from
 *
 *  @blockquote
 *  "Monte Carlo Methods in Financial Engineering", Paul Glasserman,
 *  2003, XIII, 596 p, Stochastic Modelling and Applied Probability, Vol. 53,
 *  ISBN 978-0-387-21617-1, p 124, Fig. 3.5.
 *  @endblockquote
 */
template <typename RealType = double>
class non_central_chi_squared_distribution {
public:
    typedef RealType result_type;
    typedef RealType input_type;

    class param_type {
    public:
        typedef non_central_chi_squared_distribution distribution_type;

        /**
         * Constructs the parameters of a non_central_chi_squared_distribution.
         * @c k and @c lambda are the parameter of the distribution.
         *
         * Requires: k > 0 && lambda > 0
         */
        explicit
        param_type(RealType k_arg = RealType(1), RealType lambda_arg = RealType(1))
        : _k(k_arg), _lambda(lambda_arg)
        {
            BOOST_ASSERT(k_arg > RealType(0));
            BOOST_ASSERT(lambda_arg > RealType(0));
        }

        /** Returns the @c k parameter of the distribution */
        RealType k() const { return _k; }

        /** Returns the @c lambda parameter of the distribution */
        RealType lambda() const { return _lambda; }

        /** Writes the parameters of the distribution to a @c std::ostream. */
        BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, param_type, parm)
        {
            os << parm._k << ' ' << parm._lambda;
            return os;
        }

        /** Reads the parameters of the distribution from a @c std::istream. */
        BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, param_type, parm)
        {
            is >> parm._k >> std::ws >> parm._lambda;
            return is;
        }

        /** Returns true if the parameters have the same values. */
        BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(param_type, lhs, rhs)
        { return lhs._k == rhs._k && lhs._lambda == rhs._lambda; }

        /** Returns true if the parameters have different values. */
        BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(param_type)

    private:
        RealType _k;
        RealType _lambda;
    };

    /**
     * Construct a @c non_central_chi_squared_distribution object. @c k and
     * @c lambda are the parameter of the distribution.
     *
     * Requires: k > 0 && lambda > 0
     */
    explicit
    non_central_chi_squared_distribution(RealType k_arg = RealType(1), RealType lambda_arg = RealType(1))
      : _param(k_arg, lambda_arg)
    {
        BOOST_ASSERT(k_arg > RealType(0));
        BOOST_ASSERT(lambda_arg > RealType(0));
    }

    /**
     * Construct a @c non_central_chi_squared_distribution object from the parameter.
     */
    explicit
    non_central_chi_squared_distribution(const param_type& parm)
      : _param( parm )
    { }

    /**
     * Returns a random variate distributed according to the
     * non central chi squared distribution specified by @c param.
     */
    template<typename URNG>
    RealType operator()(URNG& eng, const param_type& parm) const
    { return non_central_chi_squared_distribution(parm)(eng); }

    /**
     * Returns a random variate distributed according to the
     * non central chi squared distribution.
     */
    template<typename URNG>
    RealType operator()(URNG& eng)
    {
        using std::sqrt;
        if (_param.k() > 1) {
            boost::random::normal_distribution<RealType> n_dist;
            boost::random::chi_squared_distribution<RealType> c_dist(_param.k() - RealType(1));
            RealType _z = n_dist(eng);
            RealType _x = c_dist(eng);
            RealType term1 = _z + sqrt(_param.lambda());
            return term1*term1 + _x;
        }
        else {
            boost::random::poisson_distribution<> p_dist(_param.lambda()/RealType(2));
            boost::random::poisson_distribution<>::result_type _p = p_dist(eng);
            boost::random::chi_squared_distribution<RealType> c_dist(_param.k() + RealType(2)*_p);
            return c_dist(eng);
        }
    }

    /** Returns the @c k parameter of the distribution. */
    RealType k() const { return _param.k(); }

    /** Returns the @c lambda parameter of the distribution. */
    RealType lambda() const { return _param.lambda(); }

    /** Returns the parameters of the distribution. */
    param_type param() const { return _param; }

    /** Sets parameters of the distribution. */
    void param(const param_type& parm) { _param = parm; }

    /** Resets the distribution, so that subsequent uses does not depend on values already produced by it.*/
    void reset() {}

    /** Returns the smallest value that the distribution can produce. */
    RealType min BOOST_PREVENT_MACRO_SUBSTITUTION() const
    { return RealType(0); }

    /** Returns the largest value that the distribution can produce. */
    RealType max BOOST_PREVENT_MACRO_SUBSTITUTION() const
    { return (std::numeric_limits<RealType>::infinity)(); }

    /** Writes the parameters of the distribution to a @c std::ostream. */
    BOOST_RANDOM_DETAIL_OSTREAM_OPERATOR(os, non_central_chi_squared_distribution, dist)
    {
        os << dist.param();
        return os;
    }

    /** reads the parameters of the distribution from a @c std::istream. */
    BOOST_RANDOM_DETAIL_ISTREAM_OPERATOR(is, non_central_chi_squared_distribution, dist)
    {
        param_type parm;
        if(is >> parm) {
            dist.param(parm);
        }
        return is;
    }

    /** Returns true if two distributions have the same parameters and produce
        the same sequence of random numbers given equal generators.*/
    BOOST_RANDOM_DETAIL_EQUALITY_OPERATOR(non_central_chi_squared_distribution, lhs, rhs)
    { return lhs.param() == rhs.param(); }

    /** Returns true if two distributions have different parameters and/or can produce
       different sequences of random numbers given equal generators.*/
    BOOST_RANDOM_DETAIL_INEQUALITY_OPERATOR(non_central_chi_squared_distribution)

private:

    /// @cond show_private
    param_type  _param;
    /// @endcond
};

} // namespace random
} // namespace boost

#endif


/*! \file Peridigm_ElasticMaterial.cpp */

//@HEADER
// ************************************************************************
//
//                             Peridigm
//                 Copyright (2011) Sandia Corporation
//
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
// the U.S. Government retains certain rights in this software.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the Corporation nor the names of the
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY SANDIA CORPORATION "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 SANDIA CORPORATION OR THE
// 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.
//
// Questions?
// David J. Littlewood   djlittl@sandia.gov
// John A. Mitchell      jamitch@sandia.gov
// Michael L. Parks      mlparks@sandia.gov
// Stewart A. Silling    sasilli@sandia.gov
//
// ************************************************************************
//@HEADER

#include "Peridigm_ElasticMaterial.hpp"
#include "Peridigm_Field.hpp"
#include "elastic.h"
#ifdef PERIDIGM_KOKKOS
  #include "elastic_kokkos.h"
#endif
#include "material_utilities.h"
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;

PeridigmNS::ElasticMaterial::ElasticMaterial(const Teuchos::ParameterList& params)
  : Material(params),
    m_bulkModulus(0.0), m_shearModulus(0.0), m_density(0.0), m_alpha(0.0), m_horizon(0.0),
    m_applyAutomaticDifferentiationJacobian(true),
    m_applyThermalStrains(false),
    m_computePartialStress(false),
    m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()),
    m_volumeFieldId(-1), m_damageFieldId(-1), m_weightedVolumeFieldId(-1), m_dilatationFieldId(-1), m_modelCoordinatesFieldId(-1),
    m_coordinatesFieldId(-1), m_forceDensityFieldId(-1), m_partialStressFieldId(-1), m_bondDamageFieldId(-1),
    m_deltaTemperatureFieldId(-1), m_planeStrain(false), m_planeStress(false), m_damageModelFieldId(-1)
{
  //! \todo Add meaningful asserts on material properties.
  m_bulkModulus = calculateBulkModulus(params);
  m_shearModulus = calculateShearModulus(params);
  m_density = params.get<double>("Density");
  m_horizon = params.get<double>("Horizon");
  if(params.isParameter("Apply Automatic Differentiation Jacobian"))
    m_applyAutomaticDifferentiationJacobian = params.get<bool>("Apply Automatic Differentiation Jacobian");
  if(params.isParameter("Plane Stress"))
    m_planeStress = params.get<bool>("Plane Stress");
  if(params.isParameter("Plane Strain"))
    m_planeStrain = params.get<bool>("Plane Strain");

  if(params.isParameter("Thermal Expansion Coefficient")){
    m_alpha = params.get<double>("Thermal Expansion Coefficient");
    m_applyThermalStrains = true;
  }

  if(params.isParameter("Compute Partial Stress"))
    m_computePartialStress = params.get<bool>("Compute Partial Stress");

  PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
  m_volumeFieldId                  = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::CONSTANT, "Volume");
  m_damageFieldId                  = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Damage");
  m_weightedVolumeFieldId          = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::CONSTANT, "Weighted_Volume");
  m_dilatationFieldId              = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Dilatation");
  m_modelCoordinatesFieldId        = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR,      PeridigmField::CONSTANT, "Model_Coordinates");
  m_coordinatesFieldId             = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR,      PeridigmField::TWO_STEP, "Coordinates");
  m_forceDensityFieldId            = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR,      PeridigmField::TWO_STEP, "Force_Density");
  m_bondDamageFieldId              = fieldManager.getFieldId(PeridigmField::BOND,    PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Bond_Damage");
  int m_horizonFieldId             = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, 	 PeridigmField::CONSTANT, "Horizon");
  m_damageModelFieldId 			   = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::VECTOR, 	 PeridigmField::TWO_STEP, "Damage_Model_Data");

  if(m_applyThermalStrains)
    m_deltaTemperatureFieldId      = fieldManager.getFieldId(PeridigmField::NODE,    PeridigmField::SCALAR,      PeridigmField::TWO_STEP, "Temperature_Change");
  if(m_computePartialStress)
    m_partialStressFieldId         = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::FULL_TENSOR, PeridigmField::TWO_STEP, "Partial_Stress");

  m_fieldIds.push_back(m_volumeFieldId);
  m_fieldIds.push_back(m_damageFieldId);
  m_fieldIds.push_back(m_weightedVolumeFieldId);
  m_fieldIds.push_back(m_dilatationFieldId);
  m_fieldIds.push_back(m_modelCoordinatesFieldId);
  m_fieldIds.push_back(m_coordinatesFieldId);
  m_fieldIds.push_back(m_forceDensityFieldId);
  m_fieldIds.push_back(m_bondDamageFieldId);
  m_fieldIds.push_back(m_horizonFieldId);
  m_fieldIds.push_back(m_damageModelFieldId);
  if(m_applyThermalStrains)
    m_fieldIds.push_back(m_deltaTemperatureFieldId);
  if(m_computePartialStress)
    m_fieldIds.push_back(m_partialStressFieldId);
}

PeridigmNS::ElasticMaterial::~ElasticMaterial()
{
}

void
PeridigmNS::ElasticMaterial::initialize(const double dt,
                                        const int numOwnedPoints,
                                        const int* ownedIDs,
                                        const int* neighborhoodList,
                                        PeridigmNS::DataManager& dataManager)
{
  // Extract pointers to the underlying data
  double *xOverlap,  *cellVolumeOverlap, *weightedVolume;
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&xOverlap);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolumeOverlap);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  // have to be initialized here, to have no probelm with damage - no daamge model mix
  dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  MATERIAL_EVALUATION::computeWeightedVolume(xOverlap,cellVolumeOverlap,weightedVolume,numOwnedPoints,neighborhoodList,m_horizon);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

}

void
PeridigmNS::ElasticMaterial::computeForce(const double dt,
                                          const int numOwnedPoints,
                                          const int* ownedIDs,
                                          const int* neighborhoodList,
                                          PeridigmNS::DataManager& dataManager) const
{
  // Zero out the forces
  dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  if(m_computePartialStress)
    dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);

  // Extract pointers to the underlying data
  double *x, *y, *cellVolume, *weightedVolume, *dilatation, *bondDamage, *force, *deltaTemperature, *partialStress;

  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->ExtractView(&dilatation);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  dataManager.getData(m_forceDensityFieldId, PeridigmField::STEP_NP1)->ExtractView(&force);

  deltaTemperature = NULL;
  if(m_applyThermalStrains)
    dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);
  partialStress = NULL;
  if(m_computePartialStress)
    dataManager.getData(m_partialStressFieldId, PeridigmField::STEP_NP1)->ExtractView(&partialStress);

  MATERIAL_EVALUATION::computeDilatation(x,y,weightedVolume,cellVolume,bondDamage,dilatation,neighborhoodList,numOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);

#ifdef PERIDIGM_KOKKOS
  MATERIAL_EVALUATION::computeInternalForceLinearElasticKokkos(x,y,weightedVolume,cellVolume,dilatation,bondDamage,scf,force,neighborhoodList,numOwnedPoints,m_bulkModulus,m_shearModulus,m_horizon,m_alpha,deltaTemperature);
#else
  MATERIAL_EVALUATION::computeInternalForceLinearElastic(x,y,weightedVolume,cellVolume,dilatation,bondDamage,force,partialStress,neighborhoodList,numOwnedPoints,m_bulkModulus,m_shearModulus,m_horizon,m_alpha,deltaTemperature, m_planeStrain, m_planeStress);
#endif
}

void
PeridigmNS::ElasticMaterial::computeStoredElasticEnergyDensity(const double dt,
                                                               const int numOwnedPoints,
                                                               const int* ownedIDs,
                                                               const int* neighborhoodList,
                                                               PeridigmNS::DataManager& dataManager) const
{
  // This function is intended to be called from a compute class.
  // The compute class should have already created the Stored_Elastic_Energy_Density field id.
  //int storedElasticEnergyDensityFieldId = PeridigmNS::FieldManager::self().getFieldId("Stored_Elastic_Energy_Density");

  double *x, *y, *cellVolume, *weightedVolume, *dilatation, *bondDamage;
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->ExtractView(&dilatation);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  //double *storedElasticEnergyDensity;
  //dataManager.getData(storedElasticEnergyDensityFieldId, PeridigmField::STEP_NONE)->ExtractView(&storedElasticEnergyDensity);

  double *deltaTemperature = NULL;
  if(m_applyThermalStrains)
    dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);

  double *damageModel;
  dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
  int iID, iNID, numNeighbors, nodeId, neighborId;
  double omega, nodeInitialX[3], nodeCurrentX[3];
  double initialDistance, currentDistance, deviatoricExtension, neighborBondDamage;
  double nodeDilatation, alpha, temp;
//tbd plane strain/stress
  int neighborhoodListIndex(0), bondIndex(0);
  MATERIAL_EVALUATION::computeDilatation(x,y,weightedVolume,cellVolume,bondDamage,dilatation,neighborhoodList,numOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);

  for(iID=0 ; iID<numOwnedPoints ; ++iID){

    nodeId = ownedIDs[iID];
    nodeInitialX[0] = x[nodeId*3];
    nodeInitialX[1] = x[nodeId*3+1];
    nodeInitialX[2] = x[nodeId*3+2];
    nodeCurrentX[0] = y[nodeId*3];
    nodeCurrentX[1] = y[nodeId*3+1];
    nodeCurrentX[2] = y[nodeId*3+2];
    nodeDilatation = dilatation[nodeId];
    alpha = 15.0*m_shearModulus/weightedVolume[nodeId];

    temp = 0.0;

    numNeighbors = neighborhoodList[neighborhoodListIndex++];
    for(iNID=0 ; iNID<numNeighbors ; ++iNID){
      neighborId = neighborhoodList[neighborhoodListIndex++];
      neighborBondDamage = bondDamage[bondIndex++];
      initialDistance =
        distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2],
                 x[neighborId*3], x[neighborId*3+1], x[neighborId*3+2]);
      currentDistance =
        distance(nodeCurrentX[0], nodeCurrentX[1], nodeCurrentX[2],
                 y[neighborId*3], y[neighborId*3+1], y[neighborId*3+2]);
      if(m_applyThermalStrains)
      currentDistance -= m_alpha*deltaTemperature[nodeId]*initialDistance;
      deviatoricExtension = (currentDistance - initialDistance) - nodeDilatation*initialDistance/3.0;
      omega=m_OMEGA(initialDistance,m_horizon);
      temp += (1.0-neighborBondDamage)*omega*deviatoricExtension*deviatoricExtension*cellVolume[neighborId];
    }
    //storedElasticEnergyDensity[nodeId] = 0.5*m_bulkModulus*nodeDilatation*nodeDilatation + 0.5*alpha*temp;
    damageModel[3*iID] = 0.5*m_bulkModulus*nodeDilatation*nodeDilatation + 0.5*alpha*temp;
  }
}

void
PeridigmNS::ElasticMaterial::computeJacobian(const double dt,
                                             const int numOwnedPoints,
                                             const int* ownedIDs,
                                             const int* neighborhoodList,
                                             PeridigmNS::DataManager& dataManager,
                                             PeridigmNS::SerialMatrix& jacobian,
                                             PeridigmNS::Material::JacobianType jacobianType) const
{
  if(m_applyAutomaticDifferentiationJacobian){
    // Compute the Jacobian via automatic differentiation
    computeAutomaticDifferentiationJacobian(dt, numOwnedPoints, ownedIDs, neighborhoodList, dataManager, jacobian, jacobianType);
  }
  else{
    // Call the base class function, which computes the Jacobian by finite difference
    PeridigmNS::Material::computeJacobian(dt, numOwnedPoints, ownedIDs, neighborhoodList, dataManager, jacobian, jacobianType);
  }
}

void
PeridigmNS::ElasticMaterial::computeAutomaticDifferentiationJacobian(const double dt,
                                                                     const int numOwnedPoints,
                                                                     const int* ownedIDs,
                                                                     const int* neighborhoodList,
                                                                     PeridigmNS::DataManager& dataManager,
                                                                     PeridigmNS::SerialMatrix& jacobian,
                                                                     PeridigmNS::Material::JacobianType jacobianType) const
{
  // Compute contributions to the tangent matrix on an element-by-element basis

  // To reduce memory re-allocation, use static variable to store Fad types for
  // current coordinates (independent variables).
  static vector<Sacado::Fad::DFad<double> > y_AD;

  // Loop over all points.
  int neighborhoodListIndex = 0;
  for(int iID=0 ; iID<numOwnedPoints ; ++iID){

    // Create a temporary neighborhood consisting of a single point and its neighbors.
    int numNeighbors = neighborhoodList[neighborhoodListIndex++];
    int numEntries = numNeighbors+1;
    int numDof = 3*numEntries;
    vector<int> tempMyGlobalIDs(numEntries);
    // Put the node at the center of the neighborhood at the beginning of the list.
    tempMyGlobalIDs[0] = dataManager.getOwnedScalarPointMap()->GID(iID);
    vector<int> tempNeighborhoodList(numEntries);
    tempNeighborhoodList[0] = numNeighbors;
    for(int iNID=0 ; iNID<numNeighbors ; ++iNID){
      int neighborID = neighborhoodList[neighborhoodListIndex++];
      tempMyGlobalIDs[iNID+1] = dataManager.getOverlapScalarPointMap()->GID(neighborID);
      tempNeighborhoodList[iNID+1] = iNID+1;
    }

    Epetra_SerialComm serialComm;
    Teuchos::RCP<Epetra_BlockMap> tempOneDimensionalMap = Teuchos::rcp(new Epetra_BlockMap(numEntries, numEntries, &tempMyGlobalIDs[0], 1, 0, serialComm));
    Teuchos::RCP<Epetra_BlockMap> tempThreeDimensionalMap = Teuchos::rcp(new Epetra_BlockMap(numEntries, numEntries, &tempMyGlobalIDs[0], 3, 0, serialComm));
    Teuchos::RCP<Epetra_BlockMap> tempBondMap = Teuchos::rcp(new Epetra_BlockMap(1, 1, &tempMyGlobalIDs[0], numNeighbors, 0, serialComm));

    // Create a temporary DataManager containing data for this point and its neighborhood.
    PeridigmNS::DataManager tempDataManager;
    tempDataManager.setMaps(Teuchos::RCP<const Epetra_BlockMap>(),
                            tempOneDimensionalMap,
                            Teuchos::RCP<const Epetra_BlockMap>(),
                            tempThreeDimensionalMap,
                            tempBondMap);

    // The temporary data manager will have the same field specs and data as the real data manager.
    vector<int> fieldIds = dataManager.getFieldIds();
    tempDataManager.allocateData(fieldIds);
    tempDataManager.copyLocallyOwnedDataFromDataManager(dataManager);

    // Set up numOwnedPoints and ownedIDs.
    // There is only one owned ID, and it has local ID zero in the tempDataManager.
    int tempNumOwnedPoints = 1;
    vector<int> tempOwnedIDs(tempNumOwnedPoints);
    tempOwnedIDs[0] = 0;

    // Use the scratchMatrix as sub-matrix for storing tangent values prior to loading them into the global tangent matrix.
    // Resize scratchMatrix if necessary
    if(scratchMatrix.Dimension() < numDof)
      scratchMatrix.Resize(numDof);

    // Create a list of global indices for the rows/columns in the scratch matrix.
    vector<int> globalIndices(numDof);
    for(int i=0 ; i<numEntries ; ++i){
      int globalID = tempOneDimensionalMap->GID(i);
      for(int j=0 ; j<3 ; ++j)
        globalIndices[3*i+j] = 3*globalID+j;
    }

    // Extract pointers to the underlying data in the constitutiveData array.
    double *x, *y, *cellVolume, *weightedVolume, *damage, *bondDamage, *deltaTemperature;
    tempDataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
    tempDataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
    tempDataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
    tempDataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
    tempDataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    tempDataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
    deltaTemperature = NULL;
    if(m_applyThermalStrains)
      tempDataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);
    // Create arrays of Fad objects for the current coordinates, dilatation, and force density
    // Modify the existing vector of Fad objects for the current coordinates
    if((int)y_AD.size() < numDof)
      y_AD.resize(numDof);
    for(int i=0 ; i<numDof ; ++i){
      y_AD[i].diff(i, numDof);
      y_AD[i].val() = y[i];
    }
    // Create vectors of empty AD types for the dependent variables
    vector<Sacado::Fad::DFad<double> > dilatation_AD(numEntries);
    vector<Sacado::Fad::DFad<double> > force_AD(numDof);

    vector<Sacado::Fad::DFad<double> > partialStress_AD;
    Sacado::Fad::DFad<double> *partialStress_AD_Ptr = NULL;
    if(m_computePartialStress){
      partialStress_AD.resize(numDof*numDof);
      partialStress_AD_Ptr = &partialStress_AD[0];
    }

    // Evaluate the constitutive model using the AD types
    MATERIAL_EVALUATION::computeDilatation(x,&y_AD[0],weightedVolume,cellVolume,bondDamage,&dilatation_AD[0],&tempNeighborhoodList[0],tempNumOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);
    MATERIAL_EVALUATION::computeInternalForceLinearElastic(x,&y_AD[0],weightedVolume,cellVolume,&dilatation_AD[0],bondDamage,&force_AD[0],partialStress_AD_Ptr,&tempNeighborhoodList[0],tempNumOwnedPoints,m_bulkModulus,m_shearModulus,m_horizon,m_alpha,deltaTemperature);

    // Load derivative values into scratch matrix
    // Multiply by volume along the way to convert force density to force
    double value;
    for(int row=0 ; row<numDof ; ++row){
      for(int col=0 ; col<numDof ; ++col){
	value = force_AD[row].dx(col) * cellVolume[row/3];
	TEUCHOS_TEST_FOR_EXCEPT_MSG(!boost::math::isfinite(value), "**** NaN detected in ElasticMaterial::computeAutomaticDifferentiationJacobian().\n");
        scratchMatrix(row, col) = value;
      }
    }

    // Sum the values into the global tangent matrix (this is expensive).
    if (jacobianType == PeridigmNS::Material::FULL_MATRIX)
      jacobian.addValues((int)globalIndices.size(), &globalIndices[0], scratchMatrix.Data());
    else if (jacobianType == PeridigmNS::Material::BLOCK_DIAGONAL) {
      jacobian.addBlockDiagonalValues((int)globalIndices.size(), &globalIndices[0], scratchMatrix.Data());
    }
    else // unknown jacobian type
      TEUCHOS_TEST_FOR_EXCEPT_MSG(true, "**** Unknown Jacobian Type\n");
  }
}
void
PeridigmNS::ElasticMaterial::evalDilatation(const double dt,
                                                               const int numOwnedPoints,
                                                               const int* ownedIDs,
                                                               const int* neighborhoodList,
                                                               PeridigmNS::DataManager& dataManager) const
{
  double *x, *y, *cellVolume, *weightedVolume, *dilatation, *bondDamage, *damageModel, *deltaTemperature;
  dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
  dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
  dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
  dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
  dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->ExtractView(&dilatation);
  dataManager.getData(m_dilatationFieldId, PeridigmField::STEP_NP1)->PutScalar(0.0);
  dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);
  const double *m = weightedVolume;
  int iID;
  deltaTemperature = NULL;
  if(m_applyThermalStrains)
    dataManager.getData(m_deltaTemperatureFieldId, PeridigmField::STEP_NP1)->ExtractView(&deltaTemperature);

  MATERIAL_EVALUATION::computeDilatation(x,y,weightedVolume,cellVolume,bondDamage,dilatation,neighborhoodList,numOwnedPoints,m_horizon,m_OMEGA,m_alpha,deltaTemperature);
  //std::cout<<m_shearModulus / (*m)<<" "<<m_bulkModulus  / (*m)<< " " << <<std::endl;
  for(iID=0 ; iID<numOwnedPoints ; ++iID, dilatation++, m++){

        damageModel[3*iID]   = *dilatation;
        damageModel[3*iID+1] = m_bulkModulus  / (*m);
        damageModel[3*iID+2] = m_shearModulus / (*m);

  }

}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef DYNAMIC_SAMPLER_HH
#define DYNAMIC_SAMPLER_HH

#include "random.hh"
#include <functional>
#include <boost/mpl/if.hpp>

namespace graph_tool
{
using namespace std;
using namespace boost;

template <class Value>
class DynamicSampler
{
public:
    DynamicSampler() : _back(0), _n_items(0) {}

    DynamicSampler(const vector<Value>& items,
                   const vector<double>& probs)
        : _back(0), _n_items(0)
    {
        for (size_t i = 0; i < items.size(); ++i)
            insert(items[i], probs[i]);
    }

    typedef Value value_type;

    size_t get_left(size_t i)   const { return 2 * i + 1;               }
    size_t get_right(size_t i)  const { return 2 * i + 2;               }
    size_t get_parent(size_t i) const { return i > 0 ? (i - 1) / 2 : 0; }

    template <class RNG>
    const Value& sample(RNG& rng) const
    {
        uniform_real_distribution<> sample(0, _tree[0]);
        double u = sample(rng), c = 0;

        size_t pos = 0;
        while (_idx[pos] == numeric_limits<size_t>::max())
        {
            size_t l = get_left(pos);
            double a = _tree[l];
            if (u < a + c)
            {
                pos = l;
            }
            else
            {
                pos = get_right(pos);
                c += a;
            }
        }
        size_t i = _idx[pos];
        return _items[i];
    }

    size_t insert(const Value& v, double w)
    {
        size_t pos;
        if (_free.empty())
        {
            if (_back > 0)
            {
                // move parent to left leaf
                pos = get_parent(_back);
                size_t l = get_left(pos);
                _idx[l] = _idx[pos];
                _ipos[_idx[l]] = l;
                _tree[l] = _tree[pos];
                _idx[pos] = numeric_limits<size_t>::max();

                // position new item to the right
                _back = get_right(pos);
            }

            pos = _back;
            check_size(pos);

            _idx[pos] = _items.size();
            _items.push_back(v);
            _valid.push_back(true);
            _ipos.push_back(pos);
            _tree[pos] = w;
            _back++;
            check_size(_back);
        }
        else
        {
            pos = _free.back();
            auto i = _idx[pos];
            _items[i] = v;
            _valid[i] = true;
            _tree[pos] = w;
            _free.pop_back();
        }

        insert_leaf_prob(pos);
        _n_items++;
        return _idx[pos];
    }

    void remove(size_t i)
    {
        size_t pos = _ipos[i];
        remove_leaf_prob(pos);
        _free.push_back(pos);
        _items[i] = Value();
        _valid[i] = false;
        _n_items--;
    }

    void clear(bool shrink)
    {
        _items.clear();
        _ipos.clear();
        _tree.clear();
        _idx.clear();
        _free.clear();
        _valid.clear();
        if (shrink)
        {
            _items.shrink_to_fit();
            _ipos.shrink_to_fit();
            _tree.shrink_to_fit();
            _idx.shrink_to_fit();
            _free.shrink_to_fit();
            _valid.shrink_to_fit();
        }
        _back = 0;
        _n_items = 0;
    }

    void rebuild()
    {
        vector<Value> items;
        vector<double> probs;

        for (size_t i = 0; i < _tree.size(); ++i)
        {
            if (_idx[i] == numeric_limits<size_t>::max())
                continue;
            size_t j = _idx[i];
            if (!_valid[j])
                continue;
            items.push_back(_items[j]);
            probs.push_back(_tree[i]);
        }

        clear(true);

        for (size_t i = 0; i < items.size(); ++i)
            insert(items[i], probs[i]);
    }

    const Value& operator[](size_t i) const
    {
        return _items[i];
    }

    bool is_valid(size_t i) const
    {
        return ((i < _items.size()) && _valid[i]);
    }

    const auto& items() const
    {
        return _items;
    }

    auto begin() const
    {
        return _items.begin();
    }

    auto end() const
    {
        return _items.end();
    }

    size_t size() const
    {
        return _items.size();
    }

    bool empty() const
    {
        return _n_items == 0;
    }

private:

    void check_size(size_t i)
    {
        if (i >= _tree.size())
        {
            _idx.resize(i + 1, numeric_limits<size_t>::max());
            _tree.resize(i + 1, 0);
        }
    }

    void remove_leaf_prob(size_t i)
    {
        size_t parent = i;
        double w = _tree[i];
        while (parent > 0)
        {
            parent = get_parent(parent);
            _tree[parent] -= w;
        }
        _tree[i] = 0;
    }

    void insert_leaf_prob(size_t i)
    {
        size_t parent = i;
        double w = _tree[i];

        while (parent > 0)
        {
            parent = get_parent(parent);
            _tree[parent] += w;
        }
    }

    vector<Value>  _items;
    vector<size_t> _ipos;  // position of the item in the tree

    vector<double> _tree;  // tree nodes with weight sums
    vector<size_t> _idx;   // index in _items
    int _back;             // last item in tree

    vector<size_t> _free;  // empty leafs
    vector<bool> _valid;   // non-removed items
    size_t _n_items;
};

} // namespace graph_tool

#endif // DYNAMIC_SAMPLER_HH


/**
 * Orthanc - A Lightweight, RESTful DICOM Store
 * Copyright (C) 2012-2016 Sebastien Jodogne, Medical Physics
 * Department, University Hospital of Liege, Belgium
 * Copyright (C) 2017-2020 Osimis S.A., Belgium
 *
 * This program is free software: you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * In addition, as a special exception, the copyright holders of this
 * program give permission to link the code of its release with the
 * OpenSSL project's "OpenSSL" library (or with modified versions of it
 * that use the same license as the "OpenSSL" library), and distribute
 * the linked executables. You must obey the GNU General Public License
 * in all respects for all of the code used other than "OpenSSL". If you
 * modify file(s) with this exception, you may extend this exception to
 * your version of the file(s), but you are not obligated to do so. If
 * you do not wish to do so, delete this exception statement from your
 * version. If you delete this exception statement from all source files
 * in the program, then also delete it here.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 **/

#include "PrecompiledHeadersServer.h"
#include "SliceOrdering.h"

#include "../../OrthancFramework/Sources/Logging.h"
#include "../../OrthancFramework/Sources/Toolbox.h"
#include "ServerEnumerations.h"
#include "ServerIndex.h"

#include <algorithm>
#include <boost/lexical_cast.hpp>
#include <boost/noncopyable.hpp>

namespace Orthanc
{
  static bool TokenizeVector(std::vector<float>& result,
                             const std::string& value,
                             unsigned int expectedSize)
  {
    std::vector<std::string> tokens;
    Toolbox::TokenizeString(tokens, value, '\\');

    if (tokens.size() != expectedSize)
    {
      return false;
    }

    result.resize(tokens.size());

    for (size_t i = 0; i < tokens.size(); i++)
    {
      try
      {
        const std::string token = Toolbox::StripSpaces(tokens[i]);
        result[i] = boost::lexical_cast<float>(token);
      }
      catch (boost::bad_lexical_cast&)
      {
        return false;
      }
    }

    return true;
  }

  static bool TokenizeVector(std::vector<float>& result,
                             const DicomMap& map,
                             const DicomTag& tag,
                             unsigned int expectedSize)
  {
    const DicomValue* value = map.TestAndGetValue(tag);

    if (value == NULL ||
        value->IsNull() ||
        value->IsBinary())
    {
      return false;
    }
    else
    {
      return TokenizeVector(result, value->GetContent(), expectedSize);
    }
  }

  static bool IsCloseToZero(double x)
  {
    return fabs(x) < 10.0 * std::numeric_limits<float>::epsilon();
  }

  bool SliceOrdering::ComputeNormal(Vector& normal,
                                    const DicomMap& dicom)
  {
    std::vector<float> cosines;

    if (TokenizeVector(cosines, dicom, DICOM_TAG_IMAGE_ORIENTATION_PATIENT, 6))
    {
      assert(cosines.size() == 6);
      normal[0] = cosines[1] * cosines[5] - cosines[2] * cosines[4];
      normal[1] = cosines[2] * cosines[3] - cosines[0] * cosines[5];
      normal[2] = cosines[0] * cosines[4] - cosines[1] * cosines[3];
      return true;
    }
    else
    {
      return false;
    }
  }

  bool SliceOrdering::IsParallelOrOpposite(const Vector& u,
                                           const Vector& v)
  {
    // Check out "GeometryToolbox::IsParallelOrOpposite()" in Stone of
    // Orthanc for explanations
    const double u1 = u[0];
    const double u2 = u[1];
    const double u3 = u[2];
    const double normU = sqrt(u1 * u1 + u2 * u2 + u3 * u3);

    const double v1 = v[0];
    const double v2 = v[1];
    const double v3 = v[2];
    const double normV = sqrt(v1 * v1 + v2 * v2 + v3 * v3);

    if (IsCloseToZero(normU * normV))
    {
      return false;
    }
    else
    {
      const double cosAngle = (u1 * v1 + u2 * v2 + u3 * v3) / (normU * normV);

      return (IsCloseToZero(cosAngle - 1.0) ||      // Close to +1: Parallel, non-opposite
              IsCloseToZero(fabs(cosAngle) - 1.0)); // Close to -1: Parallel, opposite
    }
  }

  struct SliceOrdering::Instance : public boost::noncopyable
  {
  private:
    std::string   instanceId_;
    bool          hasPosition_;
    Vector        position_;
    bool          hasNormal_;
    Vector        normal_;
    bool          hasIndexInSeries_;
    size_t        indexInSeries_;
    unsigned int  framesCount_;

  public:
    Instance(ServerIndex& index,
             const std::string& instanceId) :
      instanceId_(instanceId),
      framesCount_(1)
    {
      DicomMap instance;
      if (!index.GetMainDicomTags(instance, instanceId, ResourceType_Instance, ResourceType_Instance))
      {
        throw OrthancException(ErrorCode_UnknownResource);
      }

      const DicomValue* frames = instance.TestAndGetValue(DICOM_TAG_NUMBER_OF_FRAMES);
      if (frames != NULL &&
          !frames->IsNull() &&
          !frames->IsBinary())
      {
        try
        {
          const std::string token = Toolbox::StripSpaces(frames->GetContent());
          framesCount_ = boost::lexical_cast<unsigned int>(token);
        }
        catch (boost::bad_lexical_cast&)
        {
        }
      }

      std::vector<float> tmp;
      hasPosition_ = TokenizeVector(tmp, instance, DICOM_TAG_IMAGE_POSITION_PATIENT, 3);

      if (hasPosition_)
      {
        position_[0] = tmp[0];
        position_[1] = tmp[1];
        position_[2] = tmp[2];
      }

      hasNormal_ = ComputeNormal(normal_, instance);

      std::string s;
      hasIndexInSeries_ = false;

      try
      {
        if (index.LookupMetadata(s, instanceId, MetadataType_Instance_IndexInSeries))
        {
          indexInSeries_ = boost::lexical_cast<size_t>(s);
          hasIndexInSeries_ = true;
        }
      }
      catch (boost::bad_lexical_cast&)
      {
      }
    }

    const std::string& GetIdentifier() const
    {
      return instanceId_;
    }

    bool HasPosition() const
    {
      return hasPosition_;
    }

    float ComputeRelativePosition(const Vector& normal) const
    {
      assert(HasPosition());
      return (normal[0] * position_[0] +
              normal[1] * position_[1] +
              normal[2] * position_[2]);
    }

    bool HasIndexInSeries() const
    {
      return hasIndexInSeries_;
    }

    size_t GetIndexInSeries() const
    {
      assert(HasIndexInSeries());
      return indexInSeries_;
    }

    unsigned int GetFramesCount() const
    {
      return framesCount_;
    }

    bool HasNormal() const
    {
      return hasNormal_;
    }

    const Vector& GetNormal() const
    {
      assert(hasNormal_);
      return normal_;
    }
  };

  class SliceOrdering::PositionComparator
  {
  private:
    const Vector&  normal_;

  public:
    explicit PositionComparator(const Vector& normal) : normal_(normal)
    {
    }

    int operator() (const Instance* a,
                    const Instance* b) const
    {
      return a->ComputeRelativePosition(normal_) < b->ComputeRelativePosition(normal_);
    }
  };

  bool SliceOrdering::IndexInSeriesComparator(const SliceOrdering::Instance* a,
                                              const SliceOrdering::Instance* b)
  {
    return a->GetIndexInSeries() < b->GetIndexInSeries();
  }

  void SliceOrdering::ComputeNormal()
  {
    DicomMap series;
    if (!index_.GetMainDicomTags(series, seriesId_, ResourceType_Series, ResourceType_Series))
    {
      throw OrthancException(ErrorCode_UnknownResource);
    }

    hasNormal_ = ComputeNormal(normal_, series);
  }

  void SliceOrdering::CreateInstances()
  {
    std::list<std::string> instancesId;
    index_.GetChildren(instancesId, seriesId_);

    instances_.reserve(instancesId.size());
    for (std::list<std::string>::const_iterator
           it = instancesId.begin(); it != instancesId.end(); ++it)
    {
      instances_.push_back(new Instance(index_, *it));
    }
  }

  bool SliceOrdering::SortUsingPositions()
  {
    if (instances_.size() <= 1)
    {
      // One single instance: It is sorted by default
      return true;
    }

    if (!hasNormal_)
    {
      return false;
    }

    for (size_t i = 0; i < instances_.size(); i++)
    {
      assert(instances_[i] != NULL);

      if (!instances_[i]->HasPosition() ||
          (instances_[i]->HasNormal() &&
           !IsParallelOrOpposite(instances_[i]->GetNormal(), normal_)))
      {
        return false;
      }
    }

    PositionComparator comparator(normal_);
    std::sort(instances_.begin(), instances_.end(), comparator);

    float a = instances_[0]->ComputeRelativePosition(normal_);
    for (size_t i = 1; i < instances_.size(); i++)
    {
      float b = instances_[i]->ComputeRelativePosition(normal_);

      if (std::fabs(b - a) <= 10.0f * std::numeric_limits<float>::epsilon())
      {
        // Not enough space between two slices along the normal of the volume
        return false;
      }

      a = b;
    }

    // This is a 3D volume
    isVolume_ = true;
    return true;
  }

  bool SliceOrdering::SortUsingIndexInSeries()
  {
    if (instances_.size() <= 1)
    {
      // One single instance: It is sorted by default
      return true;
    }

    for (size_t i = 0; i < instances_.size(); i++)
    {
      assert(instances_[i] != NULL);
      if (!instances_[i]->HasIndexInSeries())
      {
        return false;
      }
    }

    std::sort(instances_.begin(), instances_.end(), IndexInSeriesComparator);

    for (size_t i = 1; i < instances_.size(); i++)
    {
      if (instances_[i - 1]->GetIndexInSeries() == instances_[i]->GetIndexInSeries())
      {
        // The current "IndexInSeries" occurs 2 times: Not a proper ordering
        LOG(WARNING) << "This series contains 2 slices with the same index, trying to display it anyway";
        break;
      }
    }

    return true;
  }

  SliceOrdering::SliceOrdering(ServerIndex& index,
                               const std::string& seriesId) :
    index_(index),
    seriesId_(seriesId),
    isVolume_(false)
  {
    ComputeNormal();
    CreateInstances();

    if (!SortUsingPositions() &&
        !SortUsingIndexInSeries())
    {
      throw OrthancException(ErrorCode_CannotOrderSlices,
                             "Unable to order the slices of series " + seriesId);
    }
  }

  SliceOrdering::~SliceOrdering()
  {
    for (std::vector<Instance*>::iterator
           it = instances_.begin(); it != instances_.end(); ++it)
    {
      if (*it != NULL)
      {
        delete *it;
      }
    }
  }

  const std::string& SliceOrdering::GetInstanceId(size_t index) const
  {
    if (index >= instances_.size())
    {
      throw OrthancException(ErrorCode_ParameterOutOfRange);
    }
    else
    {
      return instances_[index]->GetIdentifier();
    }
  }

  unsigned int SliceOrdering::GetFramesCount(size_t index) const
  {
    if (index >= instances_.size())
    {
      throw OrthancException(ErrorCode_ParameterOutOfRange);
    }
    else
    {
      return instances_[index]->GetFramesCount();
    }
  }

  void SliceOrdering::Format(Json::Value& result) const
  {
    result = Json::objectValue;
    result["Type"] = (isVolume_ ? "Volume" : "Sequence");

    Json::Value tmp = Json::arrayValue;
    for (size_t i = 0; i < GetInstancesCount(); i++)
    {
      tmp.append(GetBasePath(ResourceType_Instance, GetInstanceId(i)) + "/file");
    }

    result["Dicom"] = tmp;

    Json::Value slicesShort = Json::arrayValue;

    tmp.clear();
    for (size_t i = 0; i < GetInstancesCount(); i++)
    {
      std::string base = GetBasePath(ResourceType_Instance, GetInstanceId(i));
      for (size_t j = 0; j < GetFramesCount(i); j++)
      {
        tmp.append(base + "/frames/" + boost::lexical_cast<std::string>(j));
      }

      Json::Value tmp2 = Json::arrayValue;
      tmp2.append(GetInstanceId(i));
      tmp2.append(0);
      tmp2.append(GetFramesCount(i));

      slicesShort.append(tmp2);
    }

    result["Slices"] = tmp;
    result["SlicesShort"] = slicesShort;
  }
}