#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/transforms.h>
#include <string>
#include <pcl/io/boost.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <Eigen/Dense>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#ifndef CUBE_ALIGNER_H
#define CUBE_ALIGNER_H

#define RAND_SCALE 100000
#define V_CT 7

#ifdef ADAPT_SIMPLEX
  #define ALPHA 1.0
  #define BETA (4.0/3.0)
  #define GAMMA (2.0/3.0)
  #define DELTA (5.0/6.0)
#else
  #define ALPHA 1.0
  #define BETA 2.0
  #define GAMMA 0.5
  #define DELTA 0.5
#endif

#define CUBE_MAX_ITER 500
//#define E_VAL 0.1
#define EPS_VAL 1200
#define MIN_CLUSTER_DENS 0.66

#ifndef MY_PI
#define MY_PI 3.141592
#endif

using namespace std;
using namespace pcl;

static bool isRandSeeded = false;

static double E_VAL = 0.2;

class CVertex
{
public:
  CVertex(){
    _alpha = 0;
    _beta = 0;
    _gamma = 0;
    _x = 0;
    _y = 0;
    _z = 0;
  }

  CVertex(double a, double b, double g, double x, double y, double z)
  {
    _alpha = a;
    _beta = b;
    _gamma = g;
    _x = x;
    _y = y;
    _z = z;
    _score = 0;
  }

  CVertex(Eigen::Matrix4f input)
  {
    double cb;
    _x = input(0,3);
    _y = input(1,3);
    _z = input(2,3);
    _beta = asin(-input(2,0));
    cb = cos(_beta);
    _alpha = asin(input(1,0)/cb);
    _gamma = asin(input(2,1)/cb);

    _score = 0;
  }

  /*CVertex(Eigen::Matrix4d input)
  {
    double cb;
    _x = input(0,3);
    _y = input(1,3);
    _z = input(2,3);
    _beta = asin(-input(2,0));
    cb = cos(_beta);
    _alpha = asin(input(1,0)/cb);
    _gamma = asin(input(2,1)/cb);

    _score = 0;
  }*/

#define RAD_SCALE 1.0

  CVertex(int i, double d, const CVertex& vert)
  {
    if (!isRandSeeded)
    {
      srand(time(NULL));
      isRandSeeded = true;
    }
    int m = i/7;
    _alpha = vert._alpha;
    _beta = vert._beta;
    _gamma = vert._gamma;
    _x = vert._x;
    _y = vert._y;
    _z = vert._z;

    switch(i%7)
    {
      case 0:
	_alpha += (1-2*m)*d/RAD_SCALE;
	break;
      case 1:
	_beta += (1-2*m)*d/RAD_SCALE;
	break;
      case 2:
	_gamma += (1-2*m)*d/RAD_SCALE;
	break;
      case 3:
	_x += (1-2*m)*d;
	break;
      case 4:
	_y += (1-2*m)*d;
	break;
      case 5:
	_z += (1-2*m)*d;
	break;
      default:
	break;
    }
    /*switch(j)
    {
      case 1:
	_alpha += d/10.0;
	break;
      case 2:
	_beta += d/10.0;
	break;
      case 3:
	_gamma += d/10.0;
	break;
      case 4:
	_x += d/10.0;
	break;
      case 5:
	_y += d/10.0;
	break;
      case 6:
	_z += d/10.0;
	break;
      default:
	break;
    }*/
    _score = 0;
  }

  CVertex(const CVertex& cg)
  {
    _alpha = cg._alpha;
    _beta = cg._beta;
    _gamma = cg._gamma;
    _x = cg._x;
    _y = cg._y;
    _z = cg._z;
    _score = 0;
  }

  CVertex(CVertex *cgArr, int count)
  {
    int i;
    for (i=0;i<count;i++)
    {
      _alpha = cgArr[i]._alpha/count;
      _beta = cgArr[i]._beta/count;
      _gamma = cgArr[i]._gamma/count;
      _x = cgArr[i]._x/count;
      _y = cgArr[i]._y/count;
      _z = cgArr[i]._z/count;
    }
    _score = 0;
  }

  CVertex(double maxRot, double maxTran)
  {
    if (!isRandSeeded)
    {
      srand(time(NULL));
      isRandSeeded = true;
    }
    _alpha = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxRot)/(double)RAND_SCALE;
    _beta = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxRot)/(double)RAND_SCALE;
    _gamma = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxRot)/(double)RAND_SCALE;
    _x = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxTran)/(double)RAND_SCALE;
    _y = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxTran)/(double)RAND_SCALE;
    _z = (((rand()% (RAND_SCALE*2))-RAND_SCALE)*maxTran)/(double)RAND_SCALE;
    _score = 0;
  }

  inline double getDist()
  {
    return sqrt(_x*_x + _y*_y + _z*_z);
  }

  inline double getMaxDiff(CVertex comp)
  {
    double diff = 0;

    if (fabs(_x-comp._x) > diff)
    {
      diff = fabs(_x-comp._x);
    }
    if (fabs(_y-comp._y) > diff)
    {
      diff = fabs(_y-comp._y);
    }
    if (fabs(_z-comp._z) > diff)
    {
      diff = fabs(_z-comp._z);
    }
    if (fabs(_alpha-comp._alpha) > diff)
    {
      diff = fabs(_alpha-comp._alpha);
    }
    if (fabs(_beta-comp._beta) > diff)
    {
      diff = fabs(_beta-comp._beta);
    }
    if (fabs(_gamma-comp._gamma) > diff)
    {
      diff = fabs(_gamma-comp._gamma);
    }

    return diff;
  }

  inline Eigen::Matrix4f toTranslation()
  {
    Eigen::Matrix4f outMat;
    double ca = cos(_alpha);
    double cb = cos(_beta);
    double cg = cos(_gamma);
    double sa = sin(_alpha);
    double sb = sin(_beta);
    double sg = sin(_gamma);

    //build the trans-rot matrix
    outMat(0,0) = ca*cb;
    outMat(0,1) = ca*sb*sg - sa*cg;
    outMat(0,2) = sa*sg - ca*sb*cg;
    outMat(1,0) = sa*cb;
    outMat(1,1) = ca*cg - sa*sb*sg;
    outMat(1,2) = sa*sb*cg - ca*sg;
    outMat(2,0) = -sb;
    outMat(2,1) = cb*sg;
    outMat(2,2) = cb*cg;
    outMat(3,0) = 0;outMat(3,1)=0;outMat(3,2)=0;
    outMat(0,3) = _x;
    outMat(1,3) = _y;
    outMat(2,3) = _z;
    outMat(3,3) = 1;

    return outMat;
  }

  inline void setScore(int score)
  {
    _score = score;
  }

  inline int getScore()
  {
    return _score;
  }

  inline static int sorter(const void *v1, const void *v2)
  {
    CVertex *cv1,*cv2;
    if (!v1 || !v2)
    {
      return 0;
    }
    cv1 = (CVertex*)v1;
    cv2 = (CVertex*)v2;

    return (cv1->_score < cv2->_score) -
      2*(cv1->_score > cv2->_score);
  }

  inline CVertex operator+(const CVertex& vIn)
  {
    CVertex outVert(this,1);
    outVert += vIn;

    return outVert;
  }

  inline CVertex operator+=(const CVertex& vIn)
  {
    _alpha += vIn._alpha;
    _beta += vIn._beta;
    _gamma += vIn._gamma;
    _x += vIn._x;
    _y += vIn._y;
    _z += vIn._z;

    return *this;
  }

  inline CVertex operator-(const CVertex& vIn)
  {
    CVertex outVert(this,1);
    outVert -= vIn;

    return outVert;
  }

  inline CVertex operator-=(const CVertex& vIn)
  {
    _alpha -= vIn._alpha;
    _beta -= vIn._beta;
    _gamma -= vIn._gamma;
    _x -= vIn._x;
    _y -= vIn._y;
    _z -= vIn._z;

    return *this;
  }

  inline CVertex operator*(float alpha)
  {
    CVertex outVert(this,1);
    outVert *= alpha;

    return outVert;
  }

  inline CVertex operator*=(float alpha)
  {
    _alpha *= alpha;
    _beta *= alpha;
    _gamma *= alpha;
    _x *= alpha;
    _y *= alpha;
    _z *= alpha;

    return *this;
  }

  inline double getX(){return _x;}
  inline double getY(){return _y;}
  inline double getZ(){return _z;}

private:
  friend std::ostream& operator<<(std::ostream &strm, const CVertex& vert);
  double _alpha;
  double _beta;
  double _gamma;
  double _x;
  double _y;
  double _z;
  int _score;
};

//add the scalar*matrix operator outside the class
inline CVertex operator*(float alpha, const CVertex& vert)
{
  CVertex outMat(vert);
  outMat *= alpha;
  return outMat;
}

//String writer
std::ostream& operator<<(std::ostream &strm, const CVertex& vert)
{
  return strm << "(" << vert._x << "," << vert._y << "," << vert._z << "," << vert._alpha << "," << vert._beta << "," << vert._gamma <<")";
}

class CubeAligner
{
public:
  typedef PointCloud<PointXYZI> Cloud;
  typedef PointCloud<PointXYZI>::Ptr CloudPtr;
  typedef PointCloud<PointXYZI>::iterator CloudItr;

  CubeAligner(CloudPtr cloudBase, double e)
  {
    CloudItr npIt = cloudBase->begin();
    _min_x=0;
    _max_x=0;
    _min_y=0;
    _max_y=0;
    _min_z=0;
    _max_z=0;
    _arrSz = 0;

    while (npIt<cloudBase->end())
    {
      if (_min_x==0)
      {
	_min_x = (*npIt).x;
	_max_x = (*npIt).x;
	_min_y = (*npIt).y;
	_max_y = (*npIt).y;
	_min_z = (*npIt).z;
	_max_z = (*npIt).z;
      }
      else
      {
	if ((*npIt).x < _min_x)
	{
	  _min_x = (*npIt).x;
	}
	if ((*npIt).x > _max_x)
	{
	  _max_x = (*npIt).x;
	}
	if ((*npIt).y < _min_y)
	{
	  _min_y = (*npIt).y;
	}
	if ((*npIt).y > _max_y)
	{
	  _max_y = (*npIt).y;
	}
	if ((*npIt).z < _min_z)
	{
	  _min_z = (*npIt).z;
	}
	if ((*npIt).z > _max_z)
	{
	  _max_z = (*npIt).z;
	}
      }
      npIt++;
    }
    int gf=0;

    do
    {
      _e = e + ((gf++) * 0.01);
      _xCount = (_max_x - _min_x)/_e;
      _yCount = (_max_y - _min_y)/_e;
      _zCount = (_max_z - _min_z)/_e;
    }
    while (_xCount*_yCount*_zCount <= 0);

    //printf("Setup cube grid for (%.2f,%.2f,%.2f)->(%.2f,%.2f,%.2f)\n"
    //"%ld grid points\n",
	   //_min_x,_min_y,_min_z,_max_x,_max_y,_max_z,arraySizeNeeded());
    _vBase = (unsigned char *)malloc(arraySizeNeeded());
    memset(_vBase,0,arraySizeNeeded());

    //printf("%d/%ld filled grid points\n",cloudToArray(cloudBase,_vBase),_xCount*_yCount*_zCount);
    cloudToArray(cloudBase,_vBase);
  }

  ~CubeAligner()
  {
    if (_vBase)
    {
      free(_vBase);
    }
  }

  inline void idxToXYZ(long idx, double *x, double *y, double *z)
  {
    int ix,iy,iz;
    ix = idx%_xCount;
    iy = (idx/_xCount)%_yCount;
    iz = idx/_xCount/_yCount;

    *x = _min_x + ix*_e;
    *y = _min_y + iy*_e;
    *z = _min_z + iz*_e;

    return;
  }

  inline long toIdx(long ix, long iy, long iz)
  {
    if (ix < 0 || ix >= _xCount ||
      iy < 0 || iy >= _yCount ||
      iz < 0 || iz >= _zCount)
      return -1;

    return ix+iy*_xCount+iz*_xCount*_yCount;
  }

  inline size_t arraySizeNeeded()
  {
    size_t totalCount;

    if (!_arrSz)
    {
      totalCount = _xCount*_yCount*_zCount;

      totalCount += (totalCount%8)?8:0;

      _arrSz = totalCount/8;
    }

    return _arrSz;
  }

  inline int cloudToArray(CloudPtr cloud, unsigned char *v)
  {
    CloudItr npIt = cloud->begin();
    long ix,iy,iz;
    int bitIdx;
    long long idx;
    size_t arrIdx;
    int ptCount = 0;
    int matchCount = 0;

    while (npIt<cloud->end())
    {
      ptCount++;
      ix = ((*npIt).x-_min_x)/_e;
      iy = ((*npIt).y-_min_y)/_e;
      iz = ((*npIt).z-_min_z)/_e;
      //ignore points outsize range
      if (ix < 0 || iy < 0 || iz < 0
	|| ix >= _xCount || iy >= _yCount || iz >= _zCount)
      {npIt++;continue;}

      matchCount++;

      idx = ix+iy*_xCount+iz*_xCount*_yCount;
      arrIdx = idx/8;
      bitIdx = idx%8;
      v[arrIdx] |= (0x80 >> bitIdx);

      npIt++;
    }

    return matchCount;
  }

  class CCluster
  {
  public:
    CCluster(CubeAligner *cube){_my_cube = cube;}

    inline void addIdx(long idx)
    {
      _my_pts.push_back(idx);
    }

    inline void addIdx(long ix, long iy, long iz)
    {
      _my_pts.push_back(_my_cube->toIdx(ix,iy,iz));
    }

    inline void calcDimensions()
    {
      std::vector<long>::iterator it;
      double x,y,z;
      double mX=0,mY=0,mZ=0; // max values
      //set origin to obsurdly large value to fin min value
      _x_orig = 1e9;
      _y_orig = 1e9;
      _z_orig = 1e9;

      for (it = _my_pts.begin();it < _my_pts.end(); it++)
      {
	_my_cube->idxToXYZ(*it,&x,&y,&z);

	if (x < _x_orig){_x_orig = x;}
	if (y < _y_orig){_y_orig = y;}
	if (z < _z_orig){_z_orig = z;}
	if (x > mX){mX = x;}
	if (y > mY){mY = y;}
	if (z > mZ){mZ = z;}
      }
      _dx = mX - _x_orig;
      _dy = mY - _y_orig;
      _dz = mZ - _z_orig;

      return;
    }

    inline bool isInside(long idx)
    {
      double x,y,z;
      _my_cube->idxToXYZ(idx,&x,&y,&z);

      if (x >= _x_orig && x <= (_x_orig+_dx) &&
	y >= _y_orig && y <= (_y_orig+_dy) &&
	z >= _z_orig && z <= (_z_orig+_dz))
      {
	return true;
      }
      else
      {
	return false;
      }
    }

    inline bool includes(long idx)
    {
      std::vector<long>::iterator it;

      for (it = _my_pts.begin(); it < _my_pts.end(); it++)
      {
	if (*it == idx)
	{
	  return true;
	}
      }

      return false;
    }

    inline PointXYZI getCOG()
    {
      std::vector<long>::iterator lIt;
      PointXYZI p1;
      double x,y,z;
      p1.x = 0;
      p1.y = 0;
      p1.z = 0;

      for (lIt = _my_pts.begin(); lIt < _my_pts.end() ; lIt++)
      {
	_my_cube->idxToXYZ(*lIt,&x,&y,&z);
	p1.x += x/_my_pts.size();
	p1.y += y/_my_pts.size();
	p1.z += z/_my_pts.size();
      }

      return p1;
    }

    inline CloudPtr toPolygon(int i)
    {
      CloudPtr cloudOut(new Cloud);
      std::vector<long>::iterator lIt;

      PointXYZI p1;
      double x,y,z;

      for (lIt = _my_pts.begin(); lIt < _my_pts.end() ; lIt++)
      {
	_my_cube->idxToXYZ(*lIt,&x,&y,&z);
	p1.x = x;
	p1.y = y;
	p1.z = z;
  p1.intensity = i;
	cloudOut->push_back(p1);
      }

//       PointXYZI p1,p2,p3,p4,p5,p6,p7,p8;
//       p1.x = _x_orig;
//       p1.y = _y_orig;
//       p1.z = _z_orig;
//       p2.x = _x_orig+_dx;
//       p2.y = _y_orig;
//       p2.z = _z_orig;
//       p3.x = _x_orig+_dx;
//       p3.y = _y_orig+_dy;
//       p3.z = _z_orig;
//       p4.x = _x_orig;
//       p4.y = _y_orig+_dy;
//       p4.z = _z_orig;
//       p5.x = _x_orig;
//       p5.y = _y_orig;
//       p5.z = _z_orig + _dz;
//       p6.x = _x_orig+_dx;
//       p6.y = _y_orig;
//       p6.z = _z_orig + _dz;
//       p7.x = _x_orig+_dx;
//       p7.y = _y_orig+_dy;
//       p7.z = _z_orig + _dz;
//       p8.x = _x_orig;
//       p8.y = _y_orig+_dy;
//       p8.z = _z_orig + _dz;
//       cloudOut->push_back(p1);
//       cloudOut->push_back(p2);
//       cloudOut->push_back(p3);
//       cloudOut->push_back(p4);
//       cloudOut->push_back(p5);
//       cloudOut->push_back(p6);
//       cloudOut->push_back(p7);
//       cloudOut->push_back(p8);

      return cloudOut;
    }

    inline int getSize(){return _my_pts.size();}

    inline double getDensity()
    {
      return ((double)_my_pts.size())/((_dx*_dy*_dz)/(_my_cube->getE3()));
    }

    inline CCluster splitMe()
    {
      CCluster newClust(_my_cube);
      CCluster backFillClust(_my_cube);

      std::vector<long>::iterator cIt;
      long ix,iy,iz;
      long z;
      long hitCount,lastHitCount = -1;
      double lastDens = 0;

      cout << "Trying to split" << endl;

      for (iz=0;iz<_my_cube->getZCount();iz++)
      {
	/*hitCount = 0;
	for (ix=0;ix<_my_cube->getXCount();ix++)
	{
	  for (iy=0;iy<_my_cube->getYCount();iy++)
	  {
	    if (_my_cube->isFilledIdx(_my_cube->toIdx(ix,iy,iz)))
	    {
	      hitCount++;
	    }
	  }
	}*/

	for (cIt = _my_pts.begin();cIt < _my_pts.end();
	      cIt++)
	{
	  z = (*cIt)/_my_cube->getXCount()/_my_cube->getYCount();
	  if (z == iz)
	  {
	    newClust.addIdx(*cIt);
	  }
	}
	if (iz > 1)
	{
	  newClust.calcDimensions();
	  if (lastDens > 0 && newClust.getDensity() < lastDens)
	  {
	    break;
	  }
	  lastDens = newClust.getDensity();
	}
      }

      newClust._my_pts.clear();
      if (iz < _my_cube->getZCount())
      {
	for (cIt = _my_pts.begin();cIt < _my_pts.end();
	      cIt++)
	{
	  z = (*cIt)/_my_cube->getXCount()/_my_cube->getYCount();
	  if (z < iz)
	  {
	    newClust.addIdx(*cIt);
	  }
	  else
	  {
	    backFillClust.addIdx(*cIt);
	  }
	}
	_my_pts.clear();
	for (cIt = backFillClust._my_pts.begin();cIt < backFillClust._my_pts.end();
	      cIt++)
	{
	  addIdx(*cIt);
	}
	backFillClust._my_pts.clear();
      }

      return newClust;
    }

    inline double getXMin(){return _x_orig;}
    inline double getYMin(){return _y_orig;}
    inline double getZMin(){return _z_orig;}
    inline double getXMax(){return _x_orig+_dx;}
    inline double getYMax(){return _y_orig+_dy;}
    inline double getZMax(){return _z_orig+_dz;}
    inline double getDX(){return _dx;}
    inline double getDY(){return _dy;}
    inline double getDZ(){return _dz;}

    inline std::vector<long> *getIdxs(){return &_my_pts;}

  private:
    std::vector<long> _my_pts;
    CubeAligner *_my_cube;
    double _x_orig;
    double _y_orig;
    double _z_orig;
    double _dx;
    double _dy;
    double _dz;
  };

  inline double getE3(){return _e*_e*_e;}

  inline void removeCluster(CCluster *cluster)
  {
    std::vector<long>::iterator clustIter;
    long idx,arrIdx;
    int bitIdx;

    for (clustIter = cluster->getIdxs()->begin();
	 clustIter < cluster->getIdxs()->end();
	 clustIter++)
    {
      idx = *clustIter;

      arrIdx = idx/8;
      bitIdx = idx%8;
      _vBase[arrIdx] ^= (0x80 >> bitIdx);
    }

    return;
  }

  inline void identifyClusters()
  {
    long ix,iy,iz,idx;
    std::vector<CCluster>::iterator clustIter;

    _clusters.clear();

    for (iz=0;iz<(-_min_z)/_e;iz++)
    {
      for (ix=0;ix<_xCount;ix++)
      {
	for (iy=0;iy<_yCount;iy++)
	{
	  idx = toIdx(ix,iy,iz);
	  if ( isFilledIdx(idx) )
	  {
	    bool found = false;
	    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
		 clustIter++)
	    {
	      if ((*clustIter).includes(idx))
	      {
		found = true;
		break;
	      }
	    }
	    if (found)
	    {
	      continue;
	    }
	    CCluster newClust((CubeAligner*)this);
	    //growClusterRadial(idx,idx,&newClust);
	    growCluster(idx,&newClust);
	    if (newClust.getSize() > 25)
	    {
	      newClust.calcDimensions();
	      _clusters.push_back(newClust);
	    }
	  }
	}
      }
    }

    splitClusters();

    return;

    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
	  clustIter++)
    {
      //if ((*clustIter).getZMin() < 0)
      //{
	removeCluster(&(*clustIter));
      //}
    }

    _clusters.clear();

    for (ix=0;ix<_xCount;ix++)
    {
      for (iy=0;iy<_yCount;iy++)
      {
	for (iz=0;iz<_zCount;iz++)
	{
	  idx = toIdx(ix,iy,iz);
	  if ( isFilledIdx(idx) )
	  {
	    bool found = false;
	    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
		 clustIter++)
	    {
	      if ((*clustIter).includes(idx))
	      {
		found = true;
		break;
	      }
	    }
	    if (found)
	    {
	      continue;
	    }
	    CCluster newClust((CubeAligner*)this);
	    growCluster(idx,&newClust);
	    if (newClust.getSize() > 20)
	    {
	      newClust.calcDimensions();
	      //cout << "Added Cluster -> size = " << newClust.getSize() << endl;
	      found = false;
	      for (clustIter = _clusters.begin(); clustIter < _clusters.end();
		  clustIter++)
	      {
		if (
		  (*clustIter).getDensity() < newClust.getDensity() )
		{
		  _clusters.insert(clustIter,newClust);
		  found = true;
		  break;
		}
	      }
	      if (!found)
	      {
		_clusters.push_back(newClust);
	      }
	    }
	  }
	}
      }
    }

    cout << "Found " << _clusters.size() << " Clusters" << endl;

    return;
  }

  inline std::vector<CCluster> *getClusters()
  {
    return &_clusters;
  }

  inline void splitClusters()
  {
    std::vector<CCluster>::iterator clustIter;
    for (clustIter = _clusters.begin(); clustIter < _clusters.end();
	clustIter++)
    {
      if ((*clustIter).getDensity() < MIN_CLUSTER_DENS )
      {
	_clusters.push_back((*clustIter).splitMe());
      }
    }

    return;
  }

  inline void growClusterRadial(long startIdx, long thisIdx, CCluster *clust)
  {
    long ix,iy,iz,ix2,iy2,iz2;
    int dx,dy,dz;
    long newIdx;
    bool isThisFilled;

    ix = startIdx%_xCount;
    iy = (startIdx/_xCount)%_yCount;
    iz = startIdx/_xCount/_yCount;

    isThisFilled = isFilledIdx(thisIdx);
    if (isThisFilled)
    {
      if (!clust->includes(thisIdx))
      {
	clust->addIdx(thisIdx);
      }
      else
      {
	return;
      }
    }

    if (startIdx==thisIdx)
    {
      for (dx=-1;dx<=1;dx++)
      {
	for (dy=-1;dy<=1;dy++)
	{
	  if (!dx && !dy){continue;}
	  newIdx = toIdx(ix+dx,iy+dy,iz);
	  if (newIdx >=  0)
	    growClusterRadial(startIdx,newIdx,clust);
	}
      }
    }
    else
    {
      ix2 = thisIdx%_xCount;
      iy2 = (thisIdx/_xCount)%_yCount;
      iz2 = thisIdx/_xCount/_yCount;
      dx = ix2-ix;
      dy = iy2-iy;

      //cout << "checking " << ix2<<","<<iy2<<","<<iz2<<endl;
      //cout << "ctr = " << ix<<","<<iy<<","<<iz<<endl;

      if (dx)
      {
	newIdx = toIdx(ix2+(dx<0?-1:1),iy2,iz2);
	if ( newIdx >= 0 && (isFilledIdx(newIdx) || isThisFilled))
	  growClusterRadial(startIdx,newIdx,clust);
	/*if (!isFilledIdx(newIdx) && isThisFilled)
	{
	  newIdx = toIdx(ix2+(dx<0?-1:1),iy2,iz2+1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	  newIdx = toIdx(ix2+(dx<0?-1:1),iy2,iz2-1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	}*/
      }
      if (dy)
      {
	newIdx = toIdx(ix2,iy2+(dy<0?-1:1),iz2);
	if ( newIdx >= 0 && (isFilledIdx(newIdx) || isThisFilled))
	  growClusterRadial(startIdx,newIdx,clust);
	/*if (!isFilledIdx(newIdx) & isThisFilled)
	{
	  newIdx = toIdx(ix2,iy2+(dy<0?-1:1),iz2+1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	  newIdx = toIdx(ix2,iy2+(dy<0?-1:1),iz2-1);
	  if (isFilledIdx(newIdx) && !clust->includes(newIdx))
	    growClusterRadial(startIdx,newIdx,clust);
	}*/
      }
      if (dx && dy)
      {
	newIdx = toIdx(ix2+(dx<0?-1:1),iy2+(dy<0?-1:1),iz2);
	if ( newIdx >= 0 && (isFilledIdx(newIdx) || isThisFilled))
	  growClusterRadial(startIdx,newIdx,clust);
      }
    }

    return;
  }

  inline void growCluster(long startIdx, CCluster *clust)
  {
    long ix,iy,iz,idx;
    ix = startIdx%_xCount;
    iy = (startIdx/_xCount)%_yCount;
    iz = startIdx/_xCount/_yCount;
    int offset;

    clust->addIdx(startIdx);

    for (offset = -1; offset <= 1; offset += 2)
    {
      idx = toIdx(ix,iy+offset,iz);
      if (idx >= 0 && isFilledIdx(idx) && !clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      else if ((idx = toIdx(ix,iy+offset*2,iz)) >= 0 && isFilledIdx(idx) &&
	!clust->includes(idx))
      {
	growCluster(idx,clust);
      }

      idx = toIdx(ix,iy,iz+offset);
      if (idx >= 0 && isFilledIdx(idx) && !clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      else if ( (idx = toIdx(ix,iy,iz+offset*2)) >= 0 && isFilledIdx(idx) &&
      !clust->includes(idx))
      {
      growCluster(idx,clust);
      }

      //if (abs(ixStart - (ix+offset)) <= 3)
      //{
      idx = toIdx(ix+offset,iy,iz);
      if (idx >= 0 && isFilledIdx(idx) && !clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      else if ((idx = toIdx(ix+offset*2,iy,iz)) >= 0 && isFilledIdx(idx) &&
	!clust->includes(idx))
      {
	growCluster(idx,clust);
      }
      //}
    }

    return;
  }

  inline void filterCloudCubic(CloudPtr cloud)
  {
    CloudItr npIt = cloud->begin();
    CloudPtr cloud2(new Cloud);
    ExtractIndices<PointXYZI> iextract(false);
    boost::shared_ptr<vector<int> > removeIndices(new vector<int>);
    long ix,iy,iz;
    int bitIdx;
    long long idx;
    size_t arrIdx;
    int i=0;
    int dropCount=0;

    removeIndices->reserve(cloud->size());

    while (npIt<cloud->end())
    {
      ix = ((*npIt).x-_min_x)/_e;
      iy = ((*npIt).y-_min_y)/_e;
      iz = ((*npIt).z-_min_z)/_e;

      if (isFilledIdx(toIdx(ix,iy,iz)))
      {
	removeIndices->push_back(i);
	dropCount++;
      }

      npIt++;
      i++;
    }
    //cout << "before " << cloud->size();
    iextract.setInputCloud(cloud);
    iextract.setNegative(true);
    iextract.setIndices(removeIndices);
    iextract.filter(*cloud2);
    cloud->swap(*cloud2);
    //cout << "after " << cloud->size() << endl;

    //cout << "Dropped " << dropCount << " non cubic points" << endl;
    return;
  }

  inline Eigen::Matrix4f alignToMatrix(Eigen::Matrix4f mat, CloudPtr cloud,
    double *eVal)
  {
    CloudPtr transCloud;
    CVertex x0;
    CVertex xr;
    CVertex xe;
    CVertex vertArr[V_CT];
    int i,j;
    int lCount=0;
    int cubeCount;
    int loopCount = 0;
    unsigned char *vArr;
    vArr = (unsigned char *)malloc(arraySizeNeeded());
    //try an initial offset in the direction of carts motion...
    CVertex v0(mat);
    double newScore;
    int didReduct;

    while(1)
    {
      loopCount++;
      for (i=0;i<V_CT;i++)
      {
	vertArr[i] = CVertex(i,*eVal,v0);
	transCloud = CloudPtr(new Cloud);
	transformPointCloud(*cloud,*transCloud,vertArr[i].toTranslation());
	memset(vArr,0,arraySizeNeeded());
	if (i==0){
	  cubeCount = cloudToArray(transCloud,vArr);
	}
	else
	{
	  cloudToArray(transCloud,vArr);
	}

	vertArr[i].setScore(getHitCount(vArr));
      }
      qsort(vertArr,V_CT,sizeof(CVertex),CVertex::sorter);
      didReduct = 0;

      while (1)
      {

	/*for (i=0;i<V_CT;i++)
	{
	  printf("%d,",vertArr[i].getScore());
	}
	printf("\n");*/
	for (i=0;i<V_CT;i++)
	{
	  //printf("%d,",vertArr[i].getScore());
	}
	//printf("\n");

	//calculate center of mass and reflection point
	x0 = CVertex(vertArr,V_CT-1);
	xr = x0 + ALPHA*(x0-vertArr[V_CT-1]);

	//calculate reflection points score
	transCloud = CloudPtr(new Cloud);
	transformPointCloud(*cloud,*transCloud,xr.toTranslation());
	memset(vArr,0,arraySizeNeeded());
	cloudToArray(transCloud,vArr);
	xr.setScore(getHitCount(vArr));

	for (i=0;i<V_CT;i++)
	{
	  if (xr.getScore() > vertArr[i].getScore())
	  {
	    break;
	  }
	}

	//replace the worst point with reflected and continue
	if ((i > 0 && i < V_CT-1) || xr.getScore() == vertArr[0].getScore())
	{
	  vertArr[V_CT-1] = xr;
	}
	else if (i==0)
	{
	  xe = x0 + BETA*(xr-x0);
	  //calculate expansion points score
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  xe.setScore(getHitCount(vArr));
	  if (xe.getScore() > xr.getScore())
	  {
	    vertArr[V_CT-1] = xe;
	  }
	  else
	  {
	    vertArr[V_CT-1] = xr;
	  }
	}
	else if (i == (V_CT - 1))
	{
	  //outside contraction
	  xe = x0 + GAMMA*(vertArr[V_CT-1]-x0);
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  xe.setScore(getHitCount(vArr));
	  if (xe.getScore()  > xr.getScore())
	  {
	    vertArr[V_CT-1] = xe;
	  }
	  else
	  {
	    i=-1;
	  }
	}
	else
	{
	  //inside contraction
	  xe = x0 - GAMMA*(vertArr[V_CT-1]-x0);
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  xe.setScore(getHitCount(vArr));
	  if (xe.getScore()  > xr.getScore())
	  {
	    vertArr[V_CT-1] = xe;
	  }
	  else
	  {
	    i=-1;
	  }
	}

	if (i == -1)
	{
	  //reduction
	  if (didReduct == 5)
	  {
	    lCount = CUBE_MAX_ITER;
	  }
	  else
	  {
	    //reduce
	    for (i=1;i<V_CT;i++)
	    {
	      vertArr[i] = vertArr[0] + DELTA*(vertArr[i]-vertArr[0]);
	      transCloud = CloudPtr(new Cloud);
	      transformPointCloud(*cloud,*transCloud,vertArr[i].toTranslation());
	      memset(vArr,0,arraySizeNeeded());
	      cloudToArray(transCloud,vArr);
	      vertArr[i].setScore(getHitCount(vArr));
	    }

	    //cout << "REDUCED...";
	    didReduct++;
	  }
	}

	newScore = vertArr[V_CT-1].getScore();
	//printf("Next score = %d\n",vertArr[V_CT-1].getScore());

	qsort(vertArr,V_CT,sizeof(CVertex),CVertex::sorter);
	if (didReduct && i == V_CT)
	{
	  //dont quit early from std dev right after a reduction
	  newScore = vertArr[0].getScore();
	}

	//terminate the loop
	if (++lCount > CUBE_MAX_ITER)
	{
	  break;
	}
	if (newScore != vertArr[0].getScore() &&
	  getStdDev(vertArr,V_CT) < (vertArr[0].getScore()*.01))
	{
	  break;
	}
      }

      /*cout << "TESTING 123 " << loopCount << endl << endl << endl;

      //check the distance to the new and restart maybe??
      if (loopCount <= 3 && vertArr[0].getScore() < cubeCount*0.2)
      {
	lCount = 0;
	v0 = vertArr[0];
      }
      else
      {*/
	break;
      //}
    }

    x0 = vertArr[0];

    /*newScore = x0.getScore();
    for (j=0;j<10;j++)
    {
      lCount = 0;
      for (i=0;i<V_CT;i++)
      {
	xe = CVertex(i,E_VAL/100.0,x0);
	transCloud = CloudPtr(new Cloud);
	transformPointCloud(*cloud,*transCloud,xe.toTranslation());
	memset(vArr,0,arraySizeNeeded());
	cloudToArray(transCloud,vArr);
	xe.setScore(getHitCount(vArr));
	if (xe.getScore() > newScore)
	{
	  lCount = i;
	  newScore = xe.getScore();
	  xr = xe;
	}
	cout << "Offset " << i << " = " << xe.getScore() << endl;
      }
      if (lCount == 0)
      {
	break;
      }
      else
      {
	x0=xr;
      }
    }*/

    //E_VAL = ( E_VAL + x0.getMaxDiff(v0)*2 )/2.0;
    //if (E_VAL<0.05)E_VAL = 0.05;

    //*eVal = (*eVal + v0.getMaxDiff(x0))*0.5;
    //if (*eVal<0.01) *eVal = 0.01;

    //cout << "New E_VAL = " << *eVal << endl;
    //cout << "Starting point found:" << endl << v0 << endl;
    //cout << "Best trans = " << x0 << endl;

#ifdef ADAPT_SIMPLEX
    //cout << "ADAPTING!!>>!!!!!" << endl;
#endif

    free(vArr);

    return x0.toTranslation();
  }

  inline double getNextEval(Eigen::Matrix4f *guess, Eigen::Matrix4f *align)
  {
    CVertex c1(*guess);
    CVertex c2(*align);

    return c1.getMaxDiff(c2);
  }

  inline int getXCount(){return _xCount;}
  inline int getYCount(){return _yCount;}
  inline int getZCount(){return _zCount;}

private:

  inline CVertex findStartingPoint(CloudPtr cloud, double minX,
    double maxX, double minY, double maxY, double minZ, double maxZ,
    double stepSize)
  {
    CVertex best;
    CVertex test;
    CloudPtr transCloud;
    double x,y,z;
    unsigned char *vArr;
    vArr = (unsigned char *)malloc(arraySizeNeeded());

    best.setScore(0);

    for (x = minX;x<=maxX;x+=stepSize)
    {
      for (y = minY;y<=maxY;y+=stepSize)
      {
	for (z = minZ; z <= maxZ; z+=stepSize)
	{
	  test = CVertex(0,0,0,x,y,z);
	  cout << "testing " << test <<"..." << endl;
	  transCloud = CloudPtr(new Cloud);
	  transformPointCloud(*cloud,*transCloud,test.toTranslation());
	  memset(vArr,0,arraySizeNeeded());
	  cloudToArray(transCloud,vArr);
	  test.setScore(getHitCount(vArr));
	  if (test.getScore() > best.getScore())
	  {
	    best = test;
	  }
	}
      }
    }

    free(vArr);
    return best;
  }

  inline int getHitCount(unsigned char *v2)
  {
    int count = 0;
    int i,j;
    int byteCount = arraySizeNeeded();
    unsigned char cpByte;

    for (i=0;i<byteCount;i++)
    {
      cpByte = _vBase[i]&v2[i];
      for (j=0;j<8;j++)
      {
	count += ((cpByte >> j) & 0x01);
      }
    }

    return count;
  }

  inline bool isFilledIdx(long idx)
  {
    long arrIdx;
    int bitIdx;

    //catch out of range from toIdx()
    if (idx==-1)
    {
      return false;
    }

    arrIdx = idx/8;
    bitIdx = idx%8;
    return (_vBase[arrIdx] & (0x80 >> bitIdx) );
  }

  inline double getStdDev(CVertex *vertArr, int count)
  {
    double mean=0;
    double stdDev=0;
    int i;
    for (i=0;i<count;i++)
    {
      mean += vertArr[i].getScore();
    }
    mean /= count;

    for (i=0;i<count;i++)
    {
      stdDev += pow(vertArr[i].getScore()-mean,2.0)/(count-1);
    }

    stdDev = sqrt(stdDev);
    //printf("Std dev = %.2f\n",stdDev);
    return stdDev;
  }

  std::vector<CCluster> _clusters;
  double _min_x;
  double _max_x;
  double _min_y;
  double _max_y;
  double _min_z;
  double _max_z;
  double _e;
  long _xCount;
  long _yCount;
  long _zCount;
  size_t _arrSz;
  unsigned char* _vBase;
};

#endif


#pragma once

#include "conditionals.hpp"

#include <vector>
#include <boost/math/distributions/gamma.hpp>

namespace lorad {

    class ASRV {

        public:
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
            typedef std::vector<double>                 shape_refdist_t;
            typedef std::shared_ptr<shape_refdist_t>    shape_refdist_ptr_t;
#else
            typedef std::vector<double>                 ratevar_refdist_t;
            typedef std::shared_ptr<ratevar_refdist_t>  ratevar_refdist_ptr_t;
#endif
            typedef std::vector<double>                 pinvar_refdist_t;
            typedef std::shared_ptr<pinvar_refdist_t>   pinvar_refdist_ptr_t;
#endif

            typedef std::vector<double>                 rate_prob_t;
            typedef std::shared_ptr<double>             relrate_ptr_t;
#if defined(HOLDER_ETAL_PRIOR)
            typedef std::shared_ptr<double>             shape_ptr_t;
#else
            typedef std::shared_ptr<double>             ratevar_ptr_t;
#endif
            typedef std::shared_ptr<double>             pinvar_ptr_t;
            typedef std::shared_ptr<ASRV>               SharedPtr;

                                                ASRV();
            virtual                             ~ASRV();

            void                                clear();

            void                                setNumCateg(unsigned ncateg);
            unsigned                            getNumCateg() const;

#if defined(HOLDER_ETAL_PRIOR)
            void                                setShapeSharedPtr(shape_ptr_t shape);
            void                                setShape(double v);
            const shape_ptr_t                   getShapeSharedPtr() const;
            double                              getShape() const;
            void                                fixShape(bool is_fixed);
            bool                                isFixedShape() const;
#else
            void                                setRateVarSharedPtr(ratevar_ptr_t ratevar);
            void                                setRateVar(double v);
            const ratevar_ptr_t                 getRateVarSharedPtr() const;
            double                              getRateVar() const;
            void                                fixRateVar(bool is_fixed);
            bool                                isFixedRateVar() const;
#endif

            void                                setPinvarSharedPtr(pinvar_ptr_t pinvar);
            void                                setPinvar(double p);
            const pinvar_ptr_t                  getPinvarSharedPtr() const;
            double                              getPinvar() const;
            void                                fixPinvar(bool is_fixed);
            bool                                isFixedPinvar() const;

#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
            void                                setShapeRefDistParamsSharedPtr(shape_refdist_ptr_t shape_refdist_ptr);
            std::vector<double>                 getShapeRefDistParamsVect() const;
#else
            void                                setRateVarRefDistParamsSharedPtr(ratevar_refdist_ptr_t ratevar_refdist_ptr);
            std::vector<double>                 getRateVarRefDistParamsVect() const;
#endif
            void                                setPinvarRefDistParamsSharedPtr(pinvar_refdist_ptr_t pinvar_refdist_ptr);
            std::vector<double>                 getPinvarRefDistParamsVect() const;
#endif

            void                                setIsInvarModel(bool is_invar_model);
            bool                                getIsInvarModel() const;

            const double *                      getRates() const;
            const double *                      getProbs() const;

        private:

            virtual void                        recalcASRV();

            unsigned                            _num_categ;
            bool                                _invar_model;

#if defined(HOLDER_ETAL_PRIOR)
            bool                                _shape_fixed;
            shape_ptr_t                         _shape;
#else
            bool                                _ratevar_fixed;
            ratevar_ptr_t                       _ratevar;
#endif
            pinvar_ptr_t                        _pinvar;

            bool                                _pinvar_fixed;

            rate_prob_t                         _rates;
            rate_prob_t                         _probs;
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
            shape_refdist_ptr_t                 _shape_refdist;
#else
            ratevar_refdist_ptr_t               _ratevar_refdist;
#endif
            pinvar_refdist_ptr_t                _pinvar_refdist;
#endif
    };

    inline ASRV::ASRV() {
        clear();
    }

    inline ASRV::~ASRV() {
    }

    inline void ASRV::clear() {
        // Rate homogeneity is the default
        _invar_model = false;
#if defined(HOLDER_ETAL_PRIOR)
        _shape_fixed = false;
        _shape = std::make_shared<double>(1.0);
#else
        _ratevar_fixed = false;
        _ratevar = std::make_shared<double>(1.0);
#endif
        _pinvar_fixed = false;
        _pinvar = std::make_shared<double>(0.0);
        _num_categ = 1;
#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
        ASRV::shape_refdist_t tmp = {1.0, 1.0};
        _shape_refdist = std::make_shared<ASRV::shape_refdist_t>(tmp);
#else
        ASRV::ratevar_refdist_t tmp = {1.0, 1.0};
        _ratevar_refdist = std::make_shared<ASRV::ratevar_refdist_t>(tmp);
#endif
        ASRV::pinvar_refdist_t tmp2 = {1.0, 1.0};
        _pinvar_refdist = std::make_shared<ASRV::pinvar_refdist_t>(tmp2);
#endif
        recalcASRV();
    }

#if defined(HOLDER_ETAL_PRIOR)
    inline const ASRV::shape_ptr_t ASRV::getShapeSharedPtr() const {
        return _shape;
    }

    inline double ASRV::getShape() const {
        assert(_shape);
        return *_shape;
    }

    inline void ASRV::setShapeSharedPtr(shape_ptr_t shape) {
        _shape = shape;
        recalcASRV();
    }

    inline void ASRV::setShape(double v) {
        *_shape = v;
        recalcASRV();
    }

    inline void ASRV::fixShape(bool is_fixed) {
        _shape_fixed = is_fixed;
    }

    inline bool ASRV::isFixedShape() const {
        return _shape_fixed;
    }
#else
    inline const ASRV::ratevar_ptr_t ASRV::getRateVarSharedPtr() const {
        return _ratevar;
    }

    inline double ASRV::getRateVar() const {
        assert(_ratevar);
        return *_ratevar;
    }

    inline void ASRV::setRateVarSharedPtr(ratevar_ptr_t ratevar) {
        _ratevar = ratevar;
        recalcASRV();
    }

    inline void ASRV::setRateVar(double v) {
        *_ratevar = v;
        recalcASRV();
    }

    inline void ASRV::fixRateVar(bool is_fixed) {
        _ratevar_fixed = is_fixed;
    }

    inline bool ASRV::isFixedRateVar() const {
        return _ratevar_fixed;
    }
#endif

    inline const ASRV::pinvar_ptr_t ASRV::getPinvarSharedPtr() const {
        return _pinvar;
    }

    inline double ASRV::getPinvar() const {
        assert(_pinvar);
        return *_pinvar;
    }

    inline const double * ASRV::getRates() const {
        return &_rates[0];
    }

    inline const double * ASRV::getProbs() const {
        return &_probs[0];
    }

    inline bool ASRV::getIsInvarModel() const {
        return _invar_model;
    }

    inline unsigned ASRV::getNumCateg() const {
        return _num_categ;
    }

    inline void ASRV::setNumCateg(unsigned ncateg) {
        _num_categ = ncateg;
        recalcASRV();
    }

    inline void ASRV::setPinvarSharedPtr(pinvar_ptr_t pinvar) {
        _pinvar = pinvar;
        recalcASRV();
    }

    inline void ASRV::setPinvar(double p) {
        *_pinvar = p;
        recalcASRV();
    }

    inline void ASRV::setIsInvarModel(bool is_invar_model) {
        _invar_model = is_invar_model;
        recalcASRV();
    }

    inline void ASRV::fixPinvar(bool is_fixed) {
        _pinvar_fixed = is_fixed;
    }

    inline bool ASRV::isFixedPinvar() const {
        return _pinvar_fixed;
    }

    inline void ASRV::recalcASRV() {
        // This implementation assumes discrete gamma among-site rate heterogeneity
        // using a _num_categ category discrete gamma distribution with equal category
        // probabilities and Gamma density with mean 1.0 and variance _rate_var.
        // If _invar_model is true, then rate probs will sum to 1 - _pinvar rather than 1
        // and the mean rate will be 1/(1 - _pinvar) rather than 1; the rest of the invariable
        // sites component of the model is handled outside the ASRV class.

        // _num_categ, _rate_var, and _pinvar must all have been assigned in order to compute rates and probs
#if defined(HOLDER_ETAL_PRIOR)
        if ( (!_shape) || (!_num_categ) || (!_pinvar) )
            return;
#else
        if ( (!_ratevar) || (!_num_categ) || (!_pinvar) )
            return;
#endif

        double pinvar = *_pinvar;
        assert(pinvar >= 0.0);
        assert(pinvar <  1.0);

        assert(_num_categ > 0);

        double equal_prob = 1.0/_num_categ;
        double mean_rate_variable_sites = 1.0;
        if (_invar_model)
            mean_rate_variable_sites /= (1.0 - pinvar);

        _rates.assign(_num_categ, mean_rate_variable_sites);
        _probs.assign(_num_categ, equal_prob);

#if defined(HOLDER_ETAL_PRIOR)
        double gamma_shape = *_shape;
        assert(gamma_shape >= 0.0);

        if (_num_categ == 1 || gamma_shape > 1000.0)
            return;

        double alpha = gamma_shape;
        double beta = 1.0/gamma_shape;
#else
        double rate_variance = *_ratevar;
        assert(rate_variance >= 0.0);

        if (_num_categ == 1 || rate_variance == 0.0)
            return;

        double alpha = 1.0/rate_variance;
        double beta = rate_variance;
#endif

        boost::math::gamma_distribution<> my_gamma(alpha, beta);
        boost::math::gamma_distribution<> my_gamma_plus(alpha + 1.0, beta);

        double cum_upper        = 0.0;
        double cum_upper_plus   = 0.0;
        double upper            = 0.0;
        double cum_prob         = 0.0;
        for (unsigned i = 1; i <= _num_categ; ++i) {
            double cum_lower_plus       = cum_upper_plus;
            double cum_lower            = cum_upper;
            cum_prob                    += equal_prob;

            if (i < _num_categ) {
                upper                   = boost::math::quantile(my_gamma, cum_prob);
                cum_upper_plus          = boost::math::cdf(my_gamma_plus, upper);
                cum_upper               = boost::math::cdf(my_gamma, upper);
            }
            else {
                cum_upper_plus          = 1.0;
                cum_upper               = 1.0;
            }

            double numer                = cum_upper_plus - cum_lower_plus;
            double denom                = cum_upper - cum_lower;
            double r_mean               = (denom > 0.0 ? (alpha*beta*numer/denom) : 0.0);
            _rates[i-1]        = r_mean*mean_rate_variable_sites;
        }
    }

#if defined(POLGSS)
#if defined(HOLDER_ETAL_PRIOR)
    inline void ASRV::setShapeRefDistParamsSharedPtr(ASRV::shape_refdist_ptr_t shape_refdist_params_ptr) {
        if (shape_refdist_params_ptr->size() != 2)
            throw XLorad(boost::format("Expecting 2 shape reference distribution parameters and got %d") % shape_refdist_params_ptr->size());
        _shape_refdist = shape_refdist_params_ptr;
    }

    inline std::vector<double> ASRV::getShapeRefDistParamsVect() const {
        return std::vector<double>(_shape_refdist->begin(), _shape_refdist->end());
    }
#else
    inline void ASRV::setRateVarRefDistParamsSharedPtr(ASRV::ratevar_refdist_ptr_t ratevar_refdist_params_ptr) {
        if (ratevar_refdist_params_ptr->size() != 2)
            throw XLorad(boost::format("Expecting 2 rate variance reference distribution parameters and got %d") % ratevar_refdist_params_ptr->size());
        _ratevar_refdist = ratevar_refdist_params_ptr;
    }

    inline std::vector<double> ASRV::getRateVarRefDistParamsVect() const {
        return std::vector<double>(_ratevar_refdist->begin(), _ratevar_refdist->end());
    }
#endif

    inline void ASRV::setPinvarRefDistParamsSharedPtr(ASRV::pinvar_refdist_ptr_t pinvar_refdist_params_ptr) {
        if (pinvar_refdist_params_ptr->size() != 2)
            throw XLorad(boost::format("Expecting 2 pinvar reference distribution parameters and got %d") % pinvar_refdist_params_ptr->size());
        _pinvar_refdist = pinvar_refdist_params_ptr;
    }

    inline std::vector<double> ASRV::getPinvarRefDistParamsVect() const {
        return std::vector<double>(_pinvar_refdist->begin(), _pinvar_refdist->end());
    }
#endif
}


#include <cstring>
#include <fstream>
#include <unistd.h>

#include <NTL/ZZX.h>
#include <NTL/vector.h>

#include "FHE.h"
#include "timing.h"
#include "EncryptedArray.h"

/**
 *  키를 생성하여 파일로 내보내는 모듈.
 *
 *  원래 RSA 방식과는 다르게 비밀키에서 공개키를 생성하는 구조임.
 *  공개키로 암호화를 하고, 비밀키로 복호화를 함.
 *
 *  키마다 txt파일(ascii)과 bin파일의 두 파일로 내보내짐
 *  공개키 : FHEcontext와 FHEPubKey 두 객체를 내보냄
 *  비밀키 : FHEcontext, FHESecKey, FHEPubKey 세 객체를 내보냄
 *
 */
int main(int argc, char *argv[]) {

    ArgMapping amap;

    long r = 1;
    long c = 2;
    long w = 64;
    long k = 80;
    long d = 1;

    long p = 257;
    long L = 8;
    string owner = "owner";
    string dir = "data";

    amap.arg("p", p, "plaintext base");
    amap.arg("L", L, "number of levels wanted");
    amap.arg("o", owner, "owner's address");
    amap.arg("dir", dir, "save directory");
    amap.parse(argc, argv);

    // file names
    const string secretKeyBinaryFile = dir + "/secretKey/" + owner + ".bin";
    const string publicKeyBinaryFile = dir + "/publicKey/" + owner + ".bin";

    ofstream secretBinFile(secretKeyBinaryFile.c_str(), ios::binary);
    assert(secretBinFile.is_open());

    ofstream publicBinFile(publicKeyBinaryFile.c_str(), ios::binary);
    assert(publicBinFile.is_open());

    // create context
    long m = FindM(k, L, c, p, d, 0, 0);
    std::unique_ptr<FHEcontext> context(new FHEcontext(m, p, r));
    buildModChain(*context, L, c);  // Set the modulus chain

    // create key
    std::unique_ptr<FHESecKey> secKey(new FHESecKey(*context));
    FHEPubKey *pubKey = (FHEPubKey *) secKey.get();
    secKey->GenSecKey(w);
    addSome1DMatrices(*secKey);
    addFrbMatrices(*secKey);

    // Secret Bin
    cout << "\tWriting Secret Binary file " << secretKeyBinaryFile << endl;
    writeContextBaseBinary(secretBinFile, *context);
    writeContextBinary(secretBinFile, *context);
    writePubKeyBinary(secretBinFile, *pubKey);
    writeSecKeyBinary(secretBinFile, *secKey);

    // Public Bin
    cout << "\tWriting Public Binary file " << publicKeyBinaryFile << endl;
    writeContextBaseBinary(publicBinFile, *context);
    writeContextBinary(publicBinFile, *context);
    writePubKeyBinary(publicBinFile, *pubKey);

    secretBinFile.close();
    publicBinFile.close();

    cout << "createKey successful.\n\n";
}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <vw/Core/Log.h>
#include <vw/config.h>
#include <vw/Math/Matrix.h>
#include <vw/Math/Vector.h>
#include <vw/Math/Quaternion.h>
#include <vw/Camera/PinholeModel.h>
#include <vw/Camera/LensDistortion.h>

#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
#include <vw/Math/LinearAlgebra.h>
#endif

#include <algorithm>
#include <sstream>
#include <iomanip>
#include <string>

// TODO: One day remove this protobuf dependency.
#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
#include <vw/Camera/TsaiFile.pb.h>
using google::protobuf::RepeatedFieldBackInserter;
#endif

#include <boost/filesystem/convenience.hpp>
namespace fs = boost::filesystem;

using namespace vw;
using namespace camera;

PinholeModel::PinholeModel() : m_distortion(DistortPtr(new NullLensDistortion)),
                               m_camera_center(Vector3(0,0,0)),
                               m_fu(1), m_fv(1), m_cu(0), m_cv(0),
                               m_u_direction(Vector3(1,0,0)),
                               m_v_direction(Vector3(0,1,0)),
                               m_w_direction(Vector3(0,0,1)), m_pixel_pitch(1) {

  m_rotation.set_identity();
  this->rebuild_camera_matrix();
}

/// Initialize from a file on disk.
PinholeModel::PinholeModel(std::string const& filename) : m_distortion(DistortPtr(new NullLensDistortion)) {
  read(filename);
}

PinholeModel::PinholeModel(Vector3 camera_center, Matrix<double,3,3> rotation,
                           double f_u, double f_v, double c_u, double c_v,
                           Vector3 u_direction, Vector3 v_direction,
                           Vector3 w_direction,
                           LensDistortion const& distortion_model,
                           double pixel_pitch) : m_distortion(DistortPtr(distortion_model.copy())),
                                                 m_camera_center(camera_center),
                                                 m_rotation(rotation),
                                                 m_fu(f_u), m_fv(f_v), m_cu(c_u), m_cv(c_v),
                                                 m_u_direction(u_direction),
                                                 m_v_direction(v_direction),
                                                 m_w_direction(w_direction),
                                                 m_pixel_pitch(pixel_pitch) {
  this->rebuild_camera_matrix();
}

PinholeModel::PinholeModel(Vector3 camera_center, Matrix<double,3,3> rotation,
                           double f_u, double f_v, double c_u, double c_v,
                           LensDistortion const& distortion_model,
                           double pixel_pitch) : m_distortion(DistortPtr(distortion_model.copy())),
                                                 m_camera_center(camera_center),
                                                 m_rotation(rotation),
                                                 m_fu(f_u), m_fv(f_v), m_cu(c_u), m_cv(c_v),
                                                 m_u_direction(Vector3(1,0,0)),
                                                 m_v_direction(Vector3(0,1,0)),
                                                 m_w_direction(Vector3(0,0,1)),
                                                 m_pixel_pitch(pixel_pitch) {
  rebuild_camera_matrix();
}

/// Construct a basic pinhole model with no lens distortion
PinholeModel::PinholeModel(Vector3 camera_center, Matrix<double,3,3> rotation,
                           double f_u, double f_v,
                           double c_u, double c_v,
                           double pixel_pitch) : m_distortion(DistortPtr(new NullLensDistortion)),
                                                 m_camera_center(camera_center),
                                                 m_rotation(rotation),
                                                 m_fu(f_u), m_fv(f_v), m_cu(c_u), m_cv(c_v),
                                                 m_u_direction(Vector3(1,0,0)),
                                                 m_v_direction(Vector3(0,1,0)),
                                                 m_w_direction(Vector3(0,0,1)),
                                                 m_pixel_pitch(pixel_pitch) {
  rebuild_camera_matrix();
}

void PinholeModel::read(std::string const& filename) {

  // Handle deprecated .pinhole protobuf files
  fs::path filename_path( filename );
  if ( filename_path.extension() == ".pinhole" )
    return read_protobuf_file(filename);

  // Open the input file
  std::ifstream cam_file;
  cam_file.open(filename.c_str());
  if (cam_file.fail())
    vw_throw( IOErr() << "PinholeModel::read_file: Could not open file: " << filename );

  // Check for version number on the first line
  int file_version = 1; // The default version written before the 2016 changes
  std::string line;
  std::getline(cam_file, line);
  if (line.find("VERSION") != std::string::npos) {
    sscanf(line.c_str(),"VERSION_%d", &file_version); // Parse the version of the input file

    // Right now there is only one version (VERSION_3) so if we find the version
    //  we just skip it and move on to the next line.  If the version is changed,
    //  handler logic needs to be implemented here.
    std::getline(cam_file, line);
  }

  // Start parsing all the parameters from the lines.
  if (!cam_file.good() || sscanf(line.c_str(),"fu = %lf", &m_fu) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x focal length\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"fv = %lf", &m_fv) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y focal length\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"cu = %lf", &m_cu) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read x principal point\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"cv = %lf", &m_cv) != 1) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read y principal point\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"u_direction = %lf %lf %lf",
        &m_u_direction(0), &m_u_direction(1), &m_u_direction(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read u direction vector\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"v_direction = %lf %lf %lf",
        &m_v_direction(0), &m_v_direction(1), &m_v_direction(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read v direction vector\n" );
  }

  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"w_direction = %lf %lf %lf",
        &m_w_direction(0), &m_w_direction(1), &m_w_direction(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read w direction vector\n" );
  }

  // Read extrinsic parameters
  std::getline(cam_file, line);
  if (!cam_file.good() || sscanf(line.c_str(),"C = %lf %lf %lf",
        &m_camera_center(0), &m_camera_center(1), &m_camera_center(2)) != 3) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file: Could not read C (camera center) vector\n" );
  }

  std::getline(cam_file, line);
  if ( !cam_file.good() ||
       sscanf(line.c_str(), "R = %lf %lf %lf %lf %lf %lf %lf %lf %lf",
              &m_rotation(0,0), &m_rotation(0,1), &m_rotation(0,2),
              &m_rotation(1,0), &m_rotation(1,1), &m_rotation(1,2),
              &m_rotation(2,0), &m_rotation(2,1), &m_rotation(2,2)) != 9 ) {
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Could not read rotation matrix\n" );
  }

  // The pitch line does not exist in older files, so don't fail if it is missing.
  // - At this point we need to hang on to the position immediately before the
  int lens_start = cam_file.tellg();
  std::getline(cam_file, line);
  if (line.find("pitch") != std::string::npos) {
    if (!cam_file.good() || sscanf(line.c_str(),"pitch = %lf", &m_pixel_pitch) != 1) {
      cam_file.close();
      vw_throw( IOErr() << "PinholeModel::read_file(): Could not read pixel pitch\n" );
    }
    lens_start = cam_file.tellg();
    std::getline(cam_file, line); // After reading the pitch, read the next line.
  }
  else // Pixel pitch not specified, use 1.0 as the default.
  {
    if (file_version > 2){
      cam_file.close();
      vw_throw( IOErr() << "PinholeModel::read_file(): Pitch value required in this file version!\n" );
    }
    m_pixel_pitch = 1.0;
  }

  // Now that we have loaded all the parameters, update the dependend class members.
  this->rebuild_camera_matrix();

  // This creates m_distortion but we still need to read the parameters.
  bool found_name = construct_lens_distortion(line);

  if (!found_name && (file_version > 2)){
    cam_file.close();
    vw_throw( IOErr() << "PinholeModel::read_file(): Distortion name required in this file version!\n" );
  }

  // If there was no line containing the distortion model name (true for old files)
  //  then we need to back up to before the distortion parameters begin in the file.
  if (!found_name)
    cam_file.seekg(lens_start ,std::ios_base::beg);

  // The lens distortion class knows how to parse the rest of the input stream.
  m_distortion->read(cam_file);

  cam_file.close();
}

bool PinholeModel::construct_lens_distortion(std::string const& config_line) {

  // Check if the passed in string contains the string for any of the
  //  recognized lens distortion models.
  if (config_line.find(NullLensDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new NullLensDistortion());
    return true;
  }
  if (config_line.find(BrownConradyDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new BrownConradyDistortion());
    return true;
  }
  if (config_line.find(AdjustableTsaiLensDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new AdjustableTsaiLensDistortion());
    return true;
  }
  if (config_line.find(PhotometrixLensDistortion::class_name()) != std::string::npos) {
    m_distortion.reset(new PhotometrixLensDistortion());
    return true;
  }

  // TSAI is the default model.  Older files which do not have a specifier string
  //  contain TSAI parameters.
  m_distortion.reset(new TsaiLensDistortion());

  if (config_line.find(TsaiLensDistortion::class_name()) != std::string::npos)
    return true;
  else
    return false;
}

// This file type is DEPRECATED!
void PinholeModel::read_protobuf_file(std::string const& filename) {

  fs::path filename_path( filename );
  if ( filename_path.extension() != ".pinhole" )
    vw_throw( IOErr() << "Not a protobuf file extension: " << filename_path.extension() );

#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
  std::fstream input( filename.c_str(), std::ios::in | std::ios::binary );
  if ( !input )
    vw_throw( IOErr() << "Pinhole::read_file: Could not open " << filename << "\n" );
  TsaiFile file;
  if ( !file.ParseFromIstream( &input ) )
    vw_throw( IOErr() << "Pinhole::read_file: Protocol buffer failed to parse \"" << filename << "\"\n" );
  input.close();

  // Making sure protobuf seems correct
  VW_ASSERT( file.focal_length_size() == 2,
             IOErr() << "Pinhole::read_file: Unexpected amount of focal lengths." );
  VW_ASSERT( file.center_point_size() == 2,
             IOErr() << "Pinhole::read_file: Unexpected amount of center points." );
  VW_ASSERT( file.u_direction_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of u vector." );
  VW_ASSERT( file.v_direction_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of v vector." );
  VW_ASSERT( file.w_direction_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of w vector." );
  VW_ASSERT( file.camera_center_size() == 3,
             IOErr() << "Pinhole::read_file: Unexpected size of camera vector." );
  VW_ASSERT( file.camera_rotation_size() == 9,
             IOErr() << "Pinhole::read_file: Unexpected size of rotation matrix." );

  typedef VectorProxy<double,3> Vector3P;
  m_u_direction = Vector3P(file.mutable_u_direction()->mutable_data());
  m_v_direction = Vector3P(file.mutable_v_direction()->mutable_data());
  m_w_direction = Vector3P(file.mutable_w_direction()->mutable_data());
  m_camera_center = Vector3P(file.mutable_camera_center()->mutable_data());
  m_fu = file.focal_length(0);
  m_fv = file.focal_length(1);
  m_cu = file.center_point(0);
  m_cv = file.center_point(1);
  m_rotation = MatrixProxy<double,3,3>(file.mutable_camera_rotation()->mutable_data());
  m_pixel_pitch = file.pixel_pitch();

  this->rebuild_camera_matrix();

  if ( file.distortion_name() == "NULL" ) {
    VW_ASSERT( file.distortion_vector_size() == 0,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new NullLensDistortion());
  } else if ( file.distortion_name() == "TSAI" ) {
    VW_ASSERT( file.distortion_vector_size() == 4,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new TsaiLensDistortion(VectorProxy<double,4>(file.mutable_distortion_vector()->mutable_data())));
  } else if ( file.distortion_name() == "BROWNCONRADY" ) {
    VW_ASSERT( file.distortion_vector_size() == 8,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new BrownConradyDistortion(VectorProxy<double,8>(file.mutable_distortion_vector()->mutable_data())));
  } else if ( file.distortion_name() == "AdjustableTSAI" ) {
    VW_ASSERT( file.distortion_vector_size() > 3,
               IOErr() << "Pinhole::read_file: Unexpected distortion vector." );
    m_distortion.reset( new AdjustableTsaiLensDistortion(VectorProxy<double>(file.distortion_vector_size(),file.mutable_distortion_vector()->mutable_data())));
  }
#else
  // If you hit this point, you need to install Google Protobuffers to to read this file type.
  vw_throw( IOErr() << "Pinhole::write_file: Camera IO not supported without Google Protobuffers" );
#endif

}

void PinholeModel::write(std::string const& filename) const {

  // Update this field whenever there is a significant change to the file.
  // - It can be used to keep backwards compatibility with future plain text changes.
  const std::string PINHOLE_VERSION = "VERSION_3";

  // Set the path an open the output file for writing
  std::string file_path = fs::path(filename).replace_extension(".tsai").string();
  std::ofstream cam_file(file_path.c_str());
  if( !cam_file.is_open() )
    vw_throw( IOErr() << "PinholeModel::write: Could not open file: " << file_path );

  // Write the pinhole camera model parts
  //   # digits to survive double->text->double conversion
  const size_t ACCURATE_DIGITS = 17; // = std::numeric_limits<double>::max_digits10
  cam_file << std::setprecision(ACCURATE_DIGITS);
  cam_file << PINHOLE_VERSION << "\n";
  cam_file << "fu = " << m_fu << "\n";
  cam_file << "fv = " << m_fv << "\n";
  cam_file << "cu = " << m_cu << "\n";
  cam_file << "cv = " << m_cv << "\n";
  cam_file << "u_direction = " << m_u_direction[0] << " " << m_u_direction[1] << " " << m_u_direction[2] << "\n";
  cam_file << "v_direction = " << m_v_direction[0] << " " << m_v_direction[1] << " " << m_v_direction[2] << "\n";
  cam_file << "w_direction = " << m_w_direction[0] << " " << m_w_direction[1] << " " << m_w_direction[2] << "\n";
  cam_file << "C = " << m_camera_center[0] << " " << m_camera_center[1] << " " << m_camera_center[2] << "\n";
  cam_file << "R = " << m_rotation(0,0) << " " << m_rotation(0,1) << " " << m_rotation(0,2) << " " << m_rotation(1,0) << " " << m_rotation(1,1) << " " << m_rotation(1,2) << " " << m_rotation(2,0) << " " << m_rotation(2,1) << " " << m_rotation(2,2) << "\n";
  cam_file << "pitch = " << m_pixel_pitch << "\n";

  // Write the name of the distortion model, then use the distortion model
  //  << overload to write it to the file.
  cam_file << m_distortion->name() << std::endl;
  cam_file << *m_distortion;
  cam_file.close();
}

Vector2 PinholeModel::point_to_pixel(Vector3 const& point) const {

  // Multiply the pixel location by the 3x4 camera matrix.
  // - The pixel coordinate is de-homogenized by dividing by the denominator.
  double denominator = m_camera_matrix(2,0)*point(0) + m_camera_matrix(2,1)*point(1) +
                       m_camera_matrix(2,2)*point(2) + m_camera_matrix(2,3);
  Vector2 pixel = Vector2( (m_camera_matrix(0,0)*point(0) + m_camera_matrix(0,1)*point(1) +
                            m_camera_matrix(0,2)*point(2) + m_camera_matrix(0,3)           ) / denominator,
                           (m_camera_matrix(1,0)*point(0) + m_camera_matrix(1,1)*point(1) +
                            m_camera_matrix(1,2)*point(2) + m_camera_matrix(1,3)           ) / denominator);

  // Apply the lens distortion model
  // - Divide by pixel pitch to convert from metric units to pixels if the intrinsic
  //   values were not specified in pixel units (in that case m_pixel_pitch == 1.0)
  return m_distortion->distorted_coordinates(*this, pixel)/m_pixel_pitch;
}

Vector2 PinholeModel::point_to_pixel_no_distortion(Vector3 const& point) const {

  // Multiply the pixel location by the 3x4 camera matrix.
  // - The pixel coordinate is de-homogenized by dividing by the denominator.
  double denominator = m_camera_matrix(2,0)*point(0) + m_camera_matrix(2,1)*point(1) +
                       m_camera_matrix(2,2)*point(2) + m_camera_matrix(2,3);
  Vector2 pixel = Vector2( (m_camera_matrix(0,0)*point(0) + m_camera_matrix(0,1)*point(1) +
                            m_camera_matrix(0,2)*point(2) + m_camera_matrix(0,3)           ) / denominator,
                           (m_camera_matrix(1,0)*point(0) + m_camera_matrix(1,1)*point(1) +
                            m_camera_matrix(1,2)*point(2) + m_camera_matrix(1,3)           ) / denominator);

  // Divide by pixel pitch to convert from metric units to pixels if the intrinsic
  //   values were not specified in pixel units (in that case m_pixel_pitch == 1.0)
  return pixel/m_pixel_pitch;
}

bool PinholeModel::projection_valid(Vector3 const& point) const {
  // z coordinate after extrinsic transformation
  double z = m_extrinsics(2, 0)*point(0) + m_extrinsics(2, 1)*point(1) +
    m_extrinsics(2, 2)*point(2) + m_extrinsics(2,3);
  return z > 0;
}

Vector3 PinholeModel::pixel_to_vector (Vector2 const& pix) const {
  // Apply the inverse lens distortion model
  Vector2 undistorted_pix = m_distortion->undistorted_coordinates(*this, pix*m_pixel_pitch);

  // Compute the direction of the ray emanating from the camera center.
  Vector3 p(0,0,1);
  subvector(p,0,2) = undistorted_pix;
  return normalize( m_inv_camera_transform * p);
}

Vector3 PinholeModel::camera_center(Vector2 const& /*pix*/ ) const {
  return m_camera_center;
};

void PinholeModel::set_camera_center(Vector3 const& position) {
  m_camera_center = position;
  rebuild_camera_matrix();
}

Quaternion<double> PinholeModel::camera_pose(Vector2 const& /*pix*/ ) const {
  return Quaternion<double>(m_rotation);
}

void PinholeModel::set_camera_pose(Quaternion<double> const& pose) {
  m_rotation = pose.rotation_matrix();
  rebuild_camera_matrix();
}

void PinholeModel::set_camera_pose(Matrix<double,3,3> const& pose) {
  m_rotation = pose;
  rebuild_camera_matrix();
}

void PinholeModel::coordinate_frame(Vector3 &u_vec, Vector3 &v_vec, Vector3 &w_vec) const {
  u_vec = m_u_direction;
  v_vec = m_v_direction;
  w_vec = m_w_direction;
}

void PinholeModel::set_coordinate_frame(Vector3 u_vec, Vector3 v_vec, Vector3 w_vec) {
  m_u_direction = u_vec;
  m_v_direction = v_vec;
  m_w_direction = w_vec;

  rebuild_camera_matrix();
}

Vector3 PinholeModel::coordinate_frame_u_direction() const { return m_u_direction; }
Vector3 PinholeModel::coordinate_frame_v_direction() const { return m_v_direction; }
Vector3 PinholeModel::coordinate_frame_w_direction() const { return m_w_direction; }

const LensDistortion* PinholeModel::lens_distortion() const { return m_distortion.get(); };
void PinholeModel::set_lens_distortion(LensDistortion const& distortion) {
  m_distortion = distortion.copy();
}

void PinholeModel::intrinsic_parameters(double& f_u, double& f_v,
                                        double& c_u, double& c_v) const {
  f_u = m_fu;  f_v = m_fv;  c_u = m_cu;  c_v = m_cv;
}

void PinholeModel::set_intrinsic_parameters(double f_u, double f_v,
                                            double c_u, double c_v) {
  m_fu = f_u;  m_fv = f_v;  m_cu = c_u;  m_cv = c_v;
  rebuild_camera_matrix();
}

Vector2 PinholeModel::focal_length() const { return Vector2(m_fu,m_fv); }
void PinholeModel::set_focal_length(Vector2 const& f, bool rebuild ) {
  m_fu = f[0]; m_fv = f[1];
  if (rebuild) rebuild_camera_matrix();
}
Vector2 PinholeModel::point_offset() const { return Vector2(m_cu,m_cv); }
void PinholeModel::set_point_offset(Vector2 const& c, bool rebuild ) {
  m_cu = c[0]; m_cv = c[1];
  if (rebuild) rebuild_camera_matrix();
}
double PinholeModel::pixel_pitch() const { return m_pixel_pitch; }
void PinholeModel::set_pixel_pitch( double pitch ) { m_pixel_pitch = pitch; }

void PinholeModel::set_camera_matrix( Matrix<double,3,4> const& p ) {
#if defined(VW_HAVE_PKG_LAPACK) && VW_HAVE_PKG_LAPACK==1
  // Solving for camera center
  Matrix<double> cam_nullsp = nullspace(p);
  Vector<double> cam_center = select_col(cam_nullsp,0);
  cam_center /= cam_center[3];
  m_camera_center = subvector(cam_center,0,3);

  // Solving for intrinsics with RQ decomposition
  Matrix<double> M = submatrix(p,0,0,3,3);
  Matrix<double> R,Q;
  rqd( M, R, Q );
  Matrix<double> sign_fix(3,3);
  sign_fix.set_identity();
  if ( R(0,0) < 0 )
    sign_fix(0,0) = -1;
  if ( R(1,1) < 0 )
    sign_fix(1,1) = -1;
  if ( R(2,2) < 0 )
    sign_fix(2,2) = -1;
  R = R*sign_fix;
  Q = sign_fix*Q;
  R /= R(2,2);

  // Pulling out intrinsic and last extrinsic
  Matrix<double,3,3> uvwRotation;
  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;
  m_rotation = inverse(uvwRotation*Q);
  m_fu = R(0,0);
  m_fv = R(1,1);
  m_cu = R(0,2);
  m_cv = R(1,2);

  if ( fabs(R(0,1)) >= 1.2 )
    vw_out(WarningMessage,"camera") << "Significant skew not modelled by pinhole camera\n";

  // Rebuild
  rebuild_camera_matrix();
#else
  vw_throw( NoImplErr() << "PinholeModel::set_Camera_Matrix is unavailable without LAPACK" );
#endif
}

Matrix<double,3,4> PinholeModel::camera_matrix() const {
  return m_camera_matrix;
}

void PinholeModel::rebuild_camera_matrix() {

  /// The intrinsic portion of the camera matrix is stored as
  ///
  ///    [  fx   0   cx  ]
  /// K= [  0    fy  cy  ]
  ///    [  0    0   1   ]
  ///
  /// with fx, fy the focal length of the system (in horizontal and
  /// vertical pixels), and (cx, cy) the pixel coordinates of the
  /// central pixel (the principal point on the image plane).

  m_intrinsics(0,0) = m_fu;
  m_intrinsics(0,1) = 0;
  m_intrinsics(0,2) = m_cu;
  m_intrinsics(1,0) = 0;
  m_intrinsics(1,1) = m_fv;
  m_intrinsics(1,2) = m_cv;
  m_intrinsics(2,0) = 0;
  m_intrinsics(2,1) = 0;
  m_intrinsics(2,2) = 1;

  // The extrinsics are normally built as the matrix:  [ R | -R*C ].
  // To allow for user-specified coordinate frames, the
  // extrinsics are now build to include the u,v,w rotation
  //
  //               | u_0  u_1  u_2  |
  //     Extr. =   | v_0  v_1  v_2  | * [ R | -R*C]
  //               | w_0  w_1  w_2  |
  //
  // The vectors u,v, and w must be orthonormal.

  /*   check for orthonormality of u,v,w              */
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_v_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_u_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( dot_prod(m_v_direction, m_w_direction) == 0 );
  VW_LINE_ASSERT( fabs( norm_2(m_u_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_v_direction) - 1 ) < 0.001 );
  VW_LINE_ASSERT( fabs( norm_2(m_w_direction) - 1 ) < 0.001 );

  Matrix<double,3,3> uvwRotation;

  select_row(uvwRotation,0) = m_u_direction;
  select_row(uvwRotation,1) = m_v_direction;
  select_row(uvwRotation,2) = m_w_direction;

  Matrix<double,3,3> rotation_inverse = transpose(m_rotation);
  submatrix(m_extrinsics,0,0,3,3) = uvwRotation * rotation_inverse;
  select_col(m_extrinsics,3) = uvwRotation * -rotation_inverse * m_camera_center;

  m_camera_matrix = m_intrinsics * m_extrinsics;
  m_inv_camera_transform = inverse(uvwRotation*rotation_inverse) * inverse(m_intrinsics);
}

// Apply a given rotation + translation + scale transform to a pinhole camera
void PinholeModel::apply_transform(vw::Matrix3x3 const & rotation,
                                   vw::Vector3   const & translation,
                                   double                scale) {

  // Extract current parameters
  vw::Vector3 position = this->camera_center();
  vw::Quat    pose     = this->camera_pose();

  vw::Quat rotation_quaternion(rotation);

  // New position and rotation
  position = scale*rotation*position + translation;
  pose     = rotation_quaternion*pose;
  this->set_camera_center(position);
  this->set_camera_pose  (pose);
}

PinholeModel
camera::scale_camera(PinholeModel const& camera_model, float scale) {
  if (scale == 0)
    vw_throw( ArgumentErr() << "PinholeModel::scale_camera cannot have zero scale value!" );
  // Scaling the camera is easy, just update the pixel pitch to account for the new image size.
  Vector2 focal  = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  boost::shared_ptr<LensDistortion> lens = camera_model.lens_distortion()->copy();
  return PinholeModel( camera_model.camera_center(),
                       camera_model.camera_pose().rotation_matrix(),
                       focal[0], focal[1], offset[0], offset[1],
                       camera_model.coordinate_frame_u_direction(),
                       camera_model.coordinate_frame_v_direction(),
                       camera_model.coordinate_frame_w_direction(),
                       *lens,
                       camera_model.pixel_pitch()/scale);
}

PinholeModel
camera::strip_lens_distortion(PinholeModel const& camera_model) {
  Vector2 focal  = camera_model.focal_length();
  Vector2 offset = camera_model.point_offset();
  NullLensDistortion distortion;
  return PinholeModel(camera_model.camera_center(),
                      camera_model.camera_pose().rotation_matrix(),
                      focal[0], focal[1], offset[0], offset[1],
                      camera_model.coordinate_frame_u_direction(),
                      camera_model.coordinate_frame_v_direction(),
                      camera_model.coordinate_frame_w_direction(),
                      distortion, camera_model.pixel_pitch());
}

std::ostream& camera::operator<<(std::ostream& str,
                                 PinholeModel const& model) {
  str << "Pinhole camera: \n";
  str << "\tCamera Center: " << model.camera_center() << "\n";
  str << "\tRotation Matrix: " << model.camera_pose() << "\n";
  str << "\tIntrinsics:\n";
  str << "\t  focal: "       << model.focal_length() << "\n";
  str << "\t  offset: "      << model.point_offset() << "\n";
  str << "\t  pixel pitch: " << model.pixel_pitch()  << "\n";
  str << "\tu direction: " << model.coordinate_frame_u_direction() << "\n";
  str << "\tv direction: " << model.coordinate_frame_v_direction() << "\n";
  str << "\tw direction: " << model.coordinate_frame_w_direction() << "\n";
  str << "\tDistortion Model: " << model.lens_distortion()->name() << "\n";
  str << *(model.lens_distortion()); // this will be multiple lines

  return str;
}


//***********************************************************
// 07/03/2019	1.0.0	Rémi Saint-Amant   Creation
//***********************************************************
#include "LaricobiusNigrinusModel.h"
#include "ModelBase/EntryPoint.h"
#include "Basic\DegreeDays.h"
#include <boost/math/distributions/weibull.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/Rayleigh.hpp>
#include <boost/math/distributions/logistic.hpp>
#include <boost/math/distributions/exponential.hpp>

using namespace WBSF::HOURLY_DATA;
using namespace WBSF::LNF;
using namespace std;

//static const bool BEGIN_NOVEMBER = false;
//static const size_t FIRST_Y = BEGIN_NOVEMBER ? 1 : 0;

namespace WBSF
{
	static const CDegreeDays::TDailyMethod DD_METHOD = CDegreeDays::MODIFIED_ALLEN_WAVE;
	enum { ACTUAL_CDD, DATE_DD717, DIFF_DAY, NB_OUTPUTS };

	//this line link this model with the EntryPoint of the DLL
	static const bool bRegistred =
		CModelFactory::RegisterModel(CLaricobiusNigrinusModel::CreateObject);

	CLaricobiusNigrinusModel::CLaricobiusNigrinusModel()
	{
		//NB_INPUT_PARAMETER is used to determine if the dll
		//uses the same number of parameters than the model interface
		NB_INPUT_PARAMETER = -1;
		VERSION = "1.0.2 (2020)";

		//		m_start = CTRef(YEAR_NOT_INIT, JANUARY, DAY_01);
			//	m_threshold = 5.6;
				//m_sumDD = 540;

		m_bCumul = false;
		for (size_t s = 0; s < NB_STAGES; s++)
		{
			for (size_t p = 0; p < NB_RDR_PARAMS; p++)
			{
				m_RDR[s][p] = CLaricobiusNigrinusEquations::RDR[s][p];
			}
		}

		for (size_t p = 0; p < NB_OVP_PARAMS; p++)
			m_OVP[p] = CLaricobiusNigrinusEquations::OVP[p];

		for (size_t p = 0; p < NB_ADE_PARAMS; p++)
			m_ADE[p] = CLaricobiusNigrinusEquations::ADE[p];

		for (size_t p = 0; p < NB_EAS_PARAMS; p++)
			m_EAS[p] = CLaricobiusNigrinusEquations::EAS[p];
	}

	CLaricobiusNigrinusModel::~CLaricobiusNigrinusModel()
	{
	}

	//this method is call to load your parameter in your variable
	ERMsg CLaricobiusNigrinusModel::ProcessParameters(const CParameterVector& parameters)
	{
		ERMsg msg;

		size_t c = 0;

		m_bCumul = parameters[c++].GetBool();

		if (parameters.size() == 1 + NB_STAGES * NB_RDR_PARAMS + NB_OVP_PARAMS + NB_ADE_PARAMS + NB_EAS_PARAMS)
		{
			for (size_t s = 0; s < NB_STAGES; s++)
			{
				for (size_t p = 0; p < NB_RDR_PARAMS; p++)
				{
					m_RDR[s][p] = parameters[c++].GetFloat();
				}
			}

			for (size_t p = 0; p < NB_OVP_PARAMS; p++)
				m_OVP[p] = parameters[c++].GetFloat();

			for (size_t p = 0; p < NB_ADE_PARAMS; p++)
				m_ADE[p] = parameters[c++].GetFloat();

			for (size_t p = 0; p < NB_EAS_PARAMS; p++)
				m_EAS[p] = parameters[c++].GetFloat();

		}

		return msg;
	}

	/*ERMsg CLaricobiusNigrinusModel::OnExecuteAnnual()
	{
		_ASSERTE(m_weather.size() > 1);

		ERMsg msg;
		CTRef today = CTRef::GetCurrentTRef();

		CTPeriod outputPeriod = m_weather.GetEntireTPeriod(CTM::ANNUAL);
		m_output.Init(outputPeriod, NB_OUTPUTS);

		for (size_t y = 0; y < m_weather.GetNbYears(); y++)
		{
			int year = m_weather[y].GetTRef().GetYear();
			CTRef begin = CTRef(year, m_start.GetMonth(), m_start.GetDay());
			CTRef end = CTRef(year, DECEMBER, DAY_31);

			double CDD = 0;

			CTRef day717;
			double actualCDD = -999;
			CDegreeDays DD(DD_METHOD, m_threshold);

			for (CTRef d = begin; d < end; d++)
			{
				CDD += DD.GetDD(m_weather.GetDay(d));
				if (CDD >= m_sumDD && !day717.IsInit())
					day717 = d;

				if (d.as(CTM(CTM::DAILY, CTM::OVERALL_YEARS)) == today.as(CTM(CTM::DAILY, CTM::OVERALL_YEARS)))
					actualCDD = CDD;
			}

			m_output[y][ACTUAL_CDD] = actualCDD;
			if (day717.IsInit())
			{
				m_output[y][DATE_DD717] = day717.GetRef();
				m_output[y][DIFF_DAY] = (int)day717.GetJDay() - (int)today.GetJDay();
			}
		}

		return msg;
	}*/

	//This method is called to compute the solution
	ERMsg CLaricobiusNigrinusModel::OnExecuteDaily()
	{
		ERMsg msg;

		/*	if (m_weather.GetNbYears() < 2)
			{
				msg.ajoute("Laricobius nigrinus model need at least 2 years of data");
				return msg;
			}*/

		if (!m_weather.IsHourly())
			m_weather.ComputeHourlyVariables();

		//This is where the model is actually executed
		CTPeriod p = m_weather.GetEntireTPeriod(CTM(CTM::DAILY));
		m_output.Init(p, NB_STATS, 0);

		//we simulate 2 years at a time.
		//we also manager the possibility to have only one year
		for (size_t y = 0; y < m_weather.size(); y++)
		{
			ExecuteDaily(m_weather[y].GetTRef().GetYear(), m_weather, m_output);
		}

		return msg;
	}

	void CLaricobiusNigrinusModel::ExecuteDaily(int year, const CWeatherYears& weather, CModelStatVector& output)
	{
		//Create stand
		CLNFStand stand(this, m_OVP[Τᴴ¹], m_OVP[Τᴴ²]);

		//Set parameters to equation
		for (size_t s = 0; s < NB_STAGES; s++)
		{
			for (size_t p = 0; p < NB_RDR_PARAMS; p++)
			{
				stand.m_equations.m_RDR[s][p] = m_RDR[s][p];
			}
		}

		for (size_t p = 0; p < NB_OVP_PARAMS; p++)
			stand.m_equations.m_OVP[p] = m_OVP[p];

		for (size_t p = 0; p < NB_ADE_PARAMS; p++)
			stand.m_equations.m_ADE[p] = m_ADE[p];

		for (size_t p = 0; p < NB_EAS_PARAMS; p++)
			stand.m_equations.m_EAS[p] = m_EAS[p];

		stand.init(year, weather);

		//compute 30 days avg
		//stand.ComputeTavg30(year, weather);

		//Create host
		CLNFHostPtr pHost(new CLNFHost(&stand));

		pHost->m_nbMinObjects = 10;
		pHost->m_nbMaxObjects = 1000;

		pHost->Initialize<CLaricobiusNigrinus>(CInitialPopulation(CTRef(year, JANUARY, DAY_01), 0, 400, 100, -1));

		//add host to stand
		stand.m_host.push_front(pHost);

		CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

		//if have other year extend period to February
		//ASSERT(weather[year].HavePrevious());
		//if (BEGIN_NOVEMBER)
		//{
		//	p.Begin() = CTRef(year - 1, NOVEMBER, DAY_01);
		//}

		//if have other year extend period to February
		//if (weather[year].HavePrevious())
			//p.Begin() = CTRef(year - 1, JULY, DAY_01);

		//if have other year extend period to February
		if (weather[year].HaveNext())
			p.End() = CTRef(year + 1, JUNE, DAY_30);

		for (CTRef d = p.Begin(); d <= p.End(); d++)
		{
			/*if (d == CTRef(year + 1, JANUARY, DAY_01))
			{
				int gg;
					gg=0;
			}*/

			stand.Live(weather.GetDay(d));
			if (output.IsInside(d))
				stand.GetStat(d, output[d]);

			stand.AdjustPopulation();
			HxGridTestConnection();
		}

		if (m_bCumul)
		{
			//cumulative result
			for (size_t s = S_EGG; s < S_ACTIVE_ADULT; s++)
			{
				CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));

				CStatistic stat = output.GetStat(s, p);
				if (stat.IsInit() && stat[SUM] > 0)
				{
					output[0][s] = output[0][s] * 100 / stat[SUM];//when first day is not 0
					for (CTRef d = p.Begin() + 1; d <= p.End(); d++)
					{
						output[d][s] = output[d - 1][s] + output[d][s] * 100 / stat[SUM];
						_ASSERTE(!_isnan(output[d][s]));
					}
				}
			}

			/*for (size_t s = S_M_EGG; s <= S_M_DEAD_ADULT; s++)
			{
				CTPeriod p = weather[year].GetEntireTPeriod(CTM(CTM::DAILY));
				if (s >= S_M_ACTIVE_ADULT)
				{
					p.Begin() = CTRef(year, JULY, DAY_01);
					if (weather[year].HaveNext())
						p.End() = CTRef(year + 1, JUNE, DAY_30);
				}

				CStatistic stat = output.GetStat(s, p);
				if (stat.IsInit() && stat[SUM] > 0)
				{
					for (CTRef d = p.Begin() + 1; d <= p.End(); d++)
					{
						if(output.IsInside(d))
							output[d][s] = output[d - 1][s] + output[d][s] * 100 / stat[SUM];
						_ASSERTE(!_isnan(output[d][s]));
					}
				}

			}*/

		}
	}

	void CLaricobiusNigrinusModel::AddDailyResult(const StringVector& header, const StringVector& data)
	{
		ASSERT(data.size() == 5);

		CSAResult obs;
		//if (data[0] != "BlacksburgLab" && data[0] != "VictoriaLab")
		//if (data[0] != "VictoriaLab")
		//{
		CStatistic egg_creation_date;

		obs.m_ref.FromFormatedString(data[1]);
		obs.m_obs.resize(NB_INPUTS);
		for (size_t i = 0; i < NB_INPUTS; i++)
		{
			obs.m_obs[i] = stod(data[i + 2]);

			if (i == 0 && obs.m_obs[i] > -999)
				m_egg_creation_date[data[0] + "_" + to_string(obs.m_ref.GetYear())] += obs.m_ref.GetJDay();

			//if (i == 1 && obs.m_obs[i] <= -999 && stod(data[i + 5]) > -999)
				//obs.m_obs[i] = stod(data[i + 5]);//second method

			if (obs.m_obs[i] > -999)
			{
				m_nb_days[i] += obs.m_ref.GetJDay();
				m_years[i].insert(obs.m_ref.GetYear());
			}
		}

		m_SAResult.push_back(obs);

		//}

	}

	double GetSimX(size_t s, CTRef TRefO, double obs, const CModelStatVector& output)
	{
		double x = -999;

		if (obs > -999)
		{
			//if (obs > 0.01 && obs < 99.99)
			if (obs >= 100)
				obs = 99.99;//to avoid some problem of truncation

			long index = output.GetFirstIndex(s, ">=", obs, 1, CTPeriod(TRefO.GetYear(), FIRST_MONTH, FIRST_DAY, TRefO.GetYear(), LAST_MONTH, LAST_DAY));
			if (index >= 1)
			{
				double obsX1 = output.GetFirstTRef().GetJDay() + index;
				double obsX2 = output.GetFirstTRef().GetJDay() + index + 1;

				double obsY1 = output[index][s];
				double obsY2 = output[index + 1][s];
				if (obsY2 != obsY1)
				{
					double slope = (obsX2 - obsX1) / (obsY2 - obsY1);
					double obsX = obsX1 + (obs - obsY1)*slope;
					ASSERT(!_isnan(obsX) && _finite(obsX));

					x = obsX;
				}
			}
		}

		return x;
	}

	bool CLaricobiusNigrinusModel::IsParamValid()const
	{
		if (m_OVP[Τᴴ¹] >= m_OVP[Τᴴ²])
			return false;

		bool bValid = true;
		for (size_t s = 0; s <= NB_STAGES && bValid; s++)
		{
			if (s == EGG || s == LARVAE /*|| s == AESTIVAL_DIAPAUSE_ADULT*/)
			{
				CStatistic rL;
				for (double Э = 0.01; Э < 0.5; Э += 0.01)
				{
					double r = 1.0 - log((pow(Э, m_RDR[s][Ϙ]) - 1.0) / (pow(0.5, m_RDR[s][Ϙ]) - 1.0)) / m_RDR[s][к];
					if (r >= 0.4 && r <= 2.5)
						rL += 1.0 / r;//reverse for comparison
				}

				CStatistic rH;
				for (double Э = 0.51; Э < 1.0; Э += 0.01)
				{
					double r = 1.0 - log((pow(Э, m_RDR[s][Ϙ]) - 1.0) / (pow(0.5, m_RDR[s][Ϙ]) - 1.0)) / m_RDR[s][к];
					if (r >= 0.4 && r <= 2.5)
						rH += r;
				}

				if (rL.IsInit() && rH.IsInit())
					bValid = fabs(rL[SUM] - rH[SUM]) < 5.3; //in Régnière (2012) obtain a max of 5.3
				else
					bValid = false;
			}
		}

		return bValid;
	}

	void CLaricobiusNigrinusModel::CalibrateDiapauseEndTh(CStatisticXY& stat)
	{
		static const double DiapauseDuration[3][3] =
		{
			{128.1,127.9,134.0},
			{156.7,162.2,166.2},
			{194.8,203.7,-999}
		};

		static const double DiapauseDurationSD[3][3] =
		{
			{2.2,3,	3.8},
			{4.1,4.4,5.4},
			{4.2,3.4,10.8}
		};

		if (m_SAResult.size() != 8)
			return;

		for (size_t t = 0; t < 3; t++)
		{
			for (size_t dl = 0; dl < 3; dl++)
			{
				if (DiapauseDuration[t][dl] > -999)
				{
					//NbVal = 8	Bias = 0.00263	MAE = 0.95222	RMSE = 1.25691	CD = 0.99785	R² = 0.99786
					//lam0 = 15.81011 {  15.80907, 15.81142}	VM = { 0.00021,   0.00060 }
					//lam1 = 2.50857 {   2.50779, 2.50943}	VM = { 0.00021,   0.00073 }
					//lam2 = 6.64395 {   6.63745, 6.64922}	VM = { 0.00113,   0.00379 }
					//lam3 = 7.81911 {   7.80857, 7.82666}	VM = { 0.00183,   0.00492 }
					//lam_a = 0.16346 {   0.16328, 0.16369}	VM = { 0.00006,   0.00019 }
					//lam_b = 0.26484 {   0.26458, 0.26499}	VM = { 0.00007,   0.00020 }

					double T = 10 + 5 * t;
					double DL = 8 + dl * 4;
					double DD = 120.0 + (215.0 - 120.0) * 1 / (1 + exp(-(T - m_ADE[ʎ0]) / m_ADE[ʎ1]));
					double f = exp(-m_ADE[ʎa] + m_ADE[ʎb] * 1 / (1 + exp(-(DL - m_ADE[ʎ2]) / m_ADE[ʎ3])));

					stat.Add(DiapauseDuration[t][dl], DD * f);
				}
			}
		}
	}

	static const int ROUND_VAL = 4;
	CTRef CLaricobiusNigrinusModel::GetDiapauseEnd(const CWeatherYear& weather)
	{
		CTPeriod p = weather.GetEntireTPeriod(CTM(CTM::DAILY));
		//return  p.Begin() + 253;

		double sumDD = 0;
		//for (CTRef TRef = p.Begin()+172; TRef <= p.End()&& TRef<= p.Begin() + int(m_ADE[ʎ0]); TRef++)
		for (CTRef TRef = p.Begin() + int(m_ADE[ʎ0]-1); TRef <= p.End() && TRef <= p.Begin() + int(m_ADE[ʎ1]-1); TRef++)
		{
			//size_t ii = TRef - p.Begin();
			const CWeatherDay& wday = m_weather.GetDay(TRef);
			double T = wday[H_TNTX][MEAN];

			T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);
			//T = Round(max(m_ADE[ʎa], T), ROUND_VAL);

			double DD = min(0.0, T - m_ADE[ʎb]);//DD is negative

			sumDD += DD;
		}

		//boost::math::weibull_distribution<double> begin_dist(-m_ADE[ʎ2], m_ADE[ʎ3]);
//		int begin = (int)Round(m_ADE[ʎ0] + m_ADE[ʎa] * cdf(begin_dist, -sumDD), 0);

		boost::math::logistic_distribution<double> begin_dist(m_ADE[ʎ2], m_ADE[ʎ3]);
		int begin = (int)Round(m_ADE[ʎ1] + m_ADE[ʎa] * cdf(begin_dist, sumDD), 0);
		return  p.Begin() + begin;

	}

	void CLaricobiusNigrinusModel::CalibrateDiapauseEnd(const bitset<3>& test, CStatisticXY& stat)
	{
		for (size_t EVALUATE_STAGE = 0; EVALUATE_STAGE < NB_INPUTS; EVALUATE_STAGE++)
			//for (size_t j = 0; j < NB_INPUTS; j++)
		{
			if (test[EVALUATE_STAGE])
			{

				if (m_OVP[Τᴴ¹] >= m_OVP[Τᴴ²])
					return;

				if (m_SAResult.empty())
					return;

				if (!m_weather.IsHourly())
					m_weather.ComputeHourlyVariables();

				for (size_t y = 0; y < m_weather.GetNbYears(); y++)
				{
					int year = m_weather[y].GetTRef().GetYear();
					if (m_years[EVALUATE_STAGE].find(year) == m_years[EVALUATE_STAGE].end())
						continue;

					double sumDD = 0;
					vector<double> CDD;
					CTPeriod p;

					if (EVALUATE_STAGE == I_EMERGED_ADULT)
					{

						p = m_weather[year].GetEntireTPeriod(CTM(CTM::DAILY));
						CDD.resize(p.size(), 0);

						CTRef diapauseEnd = GetDiapauseEnd(m_weather[year]);
						for (CTRef TRef = diapauseEnd; TRef <= p.End(); TRef++)
						{
							const CWeatherDay& wday = m_weather.GetDay(TRef);
							double T = wday[H_TNTX][MEAN];
							T = CLaricobiusNigrinus::AdjustTLab(wday.GetWeatherStation()->m_name, NOT_INIT, wday.GetTRef(), T);

							//double DD = max(0.0, T - 4.0);//DD is negative
							//T = min(T, m_OVP[Τᴴ²]);
							double DD = max(0.0, T - m_EAS[Τᴴ]);//DD is positive

							sumDD += DD;

							size_t ii = TRef - p.Begin();
							CDD[ii] = sumDD;
						}

					}
					else
					{
						p = m_weather[year].GetEntireTPeriod(CTM(CTM::DAILY));
						//p.Begin() = GetDiapauseEnd(m_weather[year - 1]);

						CDD.resize(p.size(), 0);

						CDegreeDays DDModel(CDegreeDays::MODIFIED_ALLEN_WAVE, m_OVP[Τᴴ¹], m_OVP[Τᴴ²]);

						for (CTRef TRef = p.Begin(); TRef <= p.End(); TRef++)
						{
							const CWeatherDay& wday = m_weather.GetDay(TRef);
							size_t ii = TRef - p.Begin();
							sumDD += DDModel.GetDD(wday);
							CDD[ii] = sumDD;
						}
					}

					for (size_t i = 0; i < m_SAResult.size(); i++)
					{
						size_t ii = m_SAResult[i].m_ref - p.Begin();
						if (m_SAResult[i].m_ref.GetYear() == year && ii < CDD.size())
						{
							double obs_y = m_SAResult[i].m_obs[EVALUATE_STAGE];

							if (obs_y > -999)
							{

								double sim_y = 0;

								if (EVALUATE_STAGE == I_EMERGED_ADULT)
								{
									boost::math::logistic_distribution<double> emerged_dist(m_EAS[μ], m_EAS[ѕ]);
									//boost::math::weibull_distribution<double> emerged_dist(m_EAS[μ], m_EAS[ѕ]);
									sim_y = Round(cdf(emerged_dist, CDD[ii]) * 100, ROUND_VAL);

								}
								else
								{
									boost::math::logistic_distribution<double> create_dist(m_OVP[μ], m_OVP[ѕ]);
									//boost::math::weibull_distribution<double> create_dist(m_OVP[μ], m_OVP[ѕ]);
									sim_y = Round(cdf(create_dist, CDD[ii]) * 100, ROUND_VAL);

								}

								if (sim_y < 0.1)
									sim_y = 0;
								if (sim_y > 99.9)
									sim_y = 100;

								stat.Add(obs_y, sim_y);
							}
						}
					}
				}//for all years
			}//if
		}//for
		return;

	}

	void CLaricobiusNigrinusModel::CalibrateOviposition(CStatisticXY& stat)
	{
		if (m_SAResult.empty())
			return;

		for (size_t y = 0; y < m_weather.GetNbYears(); y++)
		{
			int year = m_weather[y].GetTRef().GetYear();
			string key = m_info.m_loc.m_ID + "_" + to_string(year);

			if (m_egg_creation_date.find(key) == m_egg_creation_date.end())
				continue;

			//CTRef emergingBegin = GetDiapauseEnd(m_weather[year-1]);

			ASSERT(m_weather[year].HavePrevious());

			if (m_weather[year].HavePrevious())
			{
				CStatistic Tmin;
				Tmin += m_weather[year - 1][NOVEMBER][H_TMIN];
				Tmin += m_weather[year - 1][DECEMBER][H_TMIN];
				Tmin += m_weather[year][JANUARY][H_TMIN];
				Tmin += m_weather[year][FEBRUARY][H_TMIN];

				CStatistic Tmean;
				Tmean += m_weather[year - 1][NOVEMBER][H_TNTX];
				Tmean += m_weather[year - 1][DECEMBER][H_TNTX];
				Tmean += m_weather[year][JANUARY][H_TNTX];
				Tmean += m_weather[year][FEBRUARY][H_TNTX];

				double obs = m_egg_creation_date.at(key);
				double sim = m_ADE[ʎa] - m_ADE[ʎb] * 1 / (1 + exp(-(Tmean[MEAN] - m_ADE[ʎ2]) / m_ADE[ʎ3]));

				if (sim < 0.1)
					sim = 0;
				if (sim > 99.9)
					sim = 100;

				stat.Add(obs, sim);
			}

		}
		return;

	}

	void CLaricobiusNigrinusModel::GetFValueDaily(CStatisticXY& stat)
	{
		bitset<3> test;
		test.reset();

		//test.set(I_EGGS);
		//test.set(I_LARVAE);
		test.set(I_EMERGED_ADULT);

		//return CalibrateDiapauseEndTh(stat);
		return CalibrateDiapauseEnd(test, stat);
		//return CalibrateOviposition(stat);

		if (!m_SAResult.empty())
		{
			if (!m_bCumul)
				m_bCumul = true;//SA always cumulative

			if (!m_weather.IsHourly())
				m_weather.ComputeHourlyVariables();

			//low and hi relative development rate must be approximatively the same
			//if (!IsParamValid())
				//return;

			for (size_t y = 0; y < m_weather.GetNbYears(); y++)
			{
				int year = m_weather[y].GetTRef().GetYear();
				if ((test[0] && m_years[I_EGGS].find(year) != m_years[I_EGGS].end()) ||
					(test[1] && m_years[I_LARVAE].find(year) != m_years[I_LARVAE].end()) ||
					(test[2] && m_years[I_EMERGED_ADULT].find(year) != m_years[I_EMERGED_ADULT].end()))
				{

					CModelStatVector output;
					CTPeriod p = m_weather[y].GetEntireTPeriod(CTM(CTM::DAILY));
					//not possible to add a second year without having problem in evaluation....
					//if (m_weather[y].HaveNext())
						//p.End() = m_weather[y + 1].GetEntireTPeriod(CTM(CTM::DAILY)).End();

					output.Init(p, NB_STATS, 0);
					ExecuteDaily(m_weather[y].GetTRef().GetYear(), m_weather, output);

					static const size_t STAT_STAGE[3] = { S_EGG, S_LARVAE, S_ACTIVE_ADULT };

					for (size_t i = 0; i < m_SAResult.size(); i++)
					{
						if (output.IsInside(m_SAResult[i].m_ref))
						{

							for (size_t j = 0; j < NB_INPUTS; j++)
							{
								if (test[j])
								{
									double obs_y = Round(m_SAResult[i].m_obs[j], ROUND_VAL);
									double sim_y = Round(output[m_SAResult[i].m_ref][STAT_STAGE[j]], ROUND_VAL);

									if (obs_y > -999)
									{
										stat.Add(obs_y, sim_y);

										double obs_x = m_SAResult[i].m_ref.GetJDay();
										double sim_x = GetSimX(STAT_STAGE[j], m_SAResult[i].m_ref, obs_y, output);

										/*if (sim_x > -999)
										{
											obs_x = Round(100 * (obs_x - m_nb_days[j][LOWEST]) / m_nb_days[j][RANGE],ROUND_VAL);
											sim_x = Round(100 * (sim_x - m_nb_days[j][LOWEST]) / m_nb_days[j][RANGE],ROUND_VAL);
											stat.Add(obs_x, sim_x);
										}*/
									}
								}
							}
						}
					}//for all results
				}//have data
			}
		}
	}
}