/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2007-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include "InsertGenerator3D.h"
#include <boost/python/overloads.hpp>

// --- System includes ---
#include <cmath>

using std::ceil;
using std::sqrt;

// --- project includes ---
#include "sphere_fitting/Sphere3DFitter.h"


InsertGenerator3D::InsertGenerator3D()
{}

/*!
  Constructor

  \param rmin minimum particle radius
  \param rmax maximum particle radius
  \param ntries max. nr. of tries to insert particle
  \param max_iter maximum iterations within the iterative solvers
  \param prec max. error in iterative solvers
*/
InsertGenerator3D::InsertGenerator3D(double rmin,double rmax,int tries,int max_iter,double prec)
{
  m_rmin=rmin;
  m_rmax=rmax;
  m_max_tries=tries;
  m_max_iter=max_iter;
  m_prec=prec;
}

/*!
  Constructor

  \param rmin minimum particle radius
  \param rmax maximum particle radius
  \param ntries max. nr. of tries to insert particle
  \param max_iter maximum iterations within the iterative solvers
  \param prec max. error in iterative solvers
  \param seed if true, initialize random number generator via time
*/
InsertGenerator3D::InsertGenerator3D(double rmin,double rmax,int tries,int max_iter,double prec, bool seed)
{
  if (seed){
    struct timeval tv;
    gettimeofday(&tv,NULL);    
    int random_seed=tv.tv_usec;
    srand(random_seed);
  }
  m_rmin=rmin;
  m_rmax=rmax;
  m_max_tries=tries;
  m_max_iter=max_iter;
  m_prec=prec;
  m_next_tag=0;
}

/*!
  Destructor
*/
InsertGenerator3D::~InsertGenerator3D()
{}


/*!
  toggle between old (pre rev.84, ansiotropic) and new (rev 84 onwards) seeding behaviour

  \param old if true -> old behaviour, if false -> new behaviour
*/
void InsertGenerator3D::setOldSeeding(bool old)
{
  m_old_seeding=old;
}

/*!
  Set tag for the next particles to be generated. Persists until changed.

  \param tag the tag
*/
void InsertGenerator3D::setNextTag(int tag)
{
  m_next_tag=tag;
}

void InsertGenerator3D::generatePacking3 (AVolume3D* vol,MNTable3D* ntable,int gid) {
  int tag=-1;
  generatePacking4(vol,ntable,gid,tag);
}

void InsertGenerator3D::generatePacking4 (AVolume3D* vol,MNTable3D* ntable,int gid, int tag=-1) {
  ShapeList* list = (ShapeList*)0;
  generatePacking(vol,ntable,gid,tag,list);
}
/*!
  generate packing with at most a given total volume of the inserted particles
  
  \param vol a pointer to the volume in which the packing is generated
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
  \param tag the tagid of particles to be inserted
  \param maxvol maximum cumulative volume of inserted particles (may not be reached)
*/
void InsertGenerator3D::generatePackingMaxVolume(AVolume3D* vol,MNTable3D* ntable,int gid, int tag, double maxvol) 
{
  this->seedParticles(vol,ntable,gid,tag);
  this->fillIn(vol,ntable,gid,tag,maxvol);
}

/*!
  generate packing
  
  \param vol a pointer to the volume in which the packing is generated
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
  \param tag the tagid of particles to be inserted
  \param list the list of shapes to be inserted
*/
void InsertGenerator3D::generatePacking (AVolume3D* vol,MNTable3D* ntable,int gid, int tag=-1, ShapeList* list = (ShapeList*)0)
{
  if(list == (ShapeList*)0) {
    if(tag==-1){
      this->seedParticles(vol,ntable,gid,(int)m_next_tag);
      this->fillIn(vol,ntable,gid,(int)m_next_tag);
    } else {
      this->seedParticles(vol,ntable,gid,tag);
      this->fillIn(vol,ntable,gid,tag);
    }
  } else {
    if(tag==-1){
      this->seedParticles(vol,ntable,gid,(int)m_next_tag,list);
      this->fillIn(vol,ntable,gid,(int)m_next_tag,list);
    } else {
      this->seedParticles(vol,ntable,gid,tag,list);
      this->fillIn(vol,ntable,gid,tag,list);
    }
  }
}

/*!
  seed the area with particles

  \param vol a pointer to the packing volume
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
  \param tag the tag for the generated particles 
*/
void InsertGenerator3D::seedParticles(AVolume3D* vol,MNTable3D* ntable,int gid, int tag)
{
  // get bounding box
  std::cout << "InsertGenerator3D::seedParticles" << std::endl;
  pair<Vector3,Vector3> bbx=vol->getBoundingBox();
  std::cout << "bbx: " << bbx.first << " - " << bbx.second << std::endl;
  double dx=(bbx.second.X()-bbx.first.X());
  double dy=(bbx.second.Y()-bbx.first.Y());
  double dz=(bbx.second.Z()-bbx.first.Z());
  // get index limits for seeding
  int imax=int(ceil(dx/(m_rmax*2.0)));
  int jmax=int(ceil(dy/(m_rmax*sqrt(3.0))));
  int kmax=int(ceil(dz/(m_rmax*2.0*sqrt(2.0/3.0))));
  // seed positions
  for(int i=0;i<=imax;i++){
    for(int j=0;j<=jmax;j++){
      for(int k=0;k<=kmax;k++){
	// get position
	// double px=bbx.first.X()+(double(i)+0.5*double(j%2)-0.25)*m_rmax*2.0;
// 	double py=bbx.first.Y()+(double(j)+(double(k%2)-0.5)/3.0)*sqrt(3.0)*m_rmax;
// 	double pz=bbx.first.Z()+(double(k)*2.0*sqrt(2.0/3.0))*m_rmax;
	double px=bbx.first.X()+((double(i)+0.5*double(j%2)+0.5*double(k%2))*m_rmax*2.0)+m_rmax+1e-5;
	double py=bbx.first.Y()+((double(j)+double(k%2)/3.0)*sqrt(3.0)*m_rmax)+m_rmax+1e-5;
	double pz=bbx.first.Z()+((double(k)*2.0*sqrt(2.0/3.0))*m_rmax)+m_rmax+1e-5;

	// get dist to egde
	double dex=(bbx.second.X()-px) < (px-bbx.first.X()) ? bbx.second.X()-px  : px-bbx.first.X();
	double dey=(bbx.second.Y()-py) < (py-bbx.first.Y()) ? bbx.second.Y()-py  : py-bbx.first.Y();
	double dez=(bbx.second.Z()-pz) < (pz-bbx.first.Z()) ? bbx.second.Z()-pz  : pz-bbx.first.Z();
	
	double de=(dex<dey) ? dex : dey;
	de = (de < dez) ? de : dez;

	// check max rad.
	if(de>m_rmin){
	  // calc random radius
	  double r;
	  double jitter;
	  if(de<m_rmax) {
	    if(m_old_seeding){
	      r=m_rmin+((de-m_rmin)*((double)(rand())/(double)(RAND_MAX)));
	      jitter=0.0;
	    } else {
	      r=m_rmin+(((de-m_rmin)/2.0)*((double)(rand())/(double)(RAND_MAX)));
	      jitter=de-r;
	    }
	  } else {
	    if(m_old_seeding){
	      r=m_rmin+((m_rmax-m_rmin)*((double)(rand())/(double)(RAND_MAX)));
	      jitter=0.0;
	    } else {
	      r=m_rmin+(((m_rmax-m_rmin)/2.0)*((double)(rand())/(double)(RAND_MAX)));
	      jitter=m_rmax-r;
	    }
	  }
	  //std::cout << "pos: " << Vector3(px,py,pz) << " r: " << r << std::endl;
	  // jitter position 
	  double dx=jitter*(2.0*((double)(rand())/(double)(RAND_MAX))-1.0);
	  double dy=jitter*(2.0*((double)(rand())/(double)(RAND_MAX))-1.0);
	  double dz=jitter*(2.0*((double)(rand())/(double)(RAND_MAX))-1.0);
	  Sphere S(Vector3(px+dx,py+dy,pz+dz),r);
	  bool fit=vol->isIn(S) && ntable->checkInsertable(S,gid);
	  if(fit){
	    S.setTag(tag);
	    ntable->insertChecked(S,gid);
	    //std::cout << "inserted" << std::endl;
	  } // else {
// 	    std::cout << "isIn " << vol->isIn(S) << " insertable: " << ntable->checkInsertable(S,gid) << std::endl;
// 	  }
	}
      }
    }
  }

}
/*!
  seed the area with particles

  \param vol a pointer to the packing volume
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
  \param tag the tag for the generated particles 
*/
void InsertGenerator3D::seedParticles(AVolume3D* vol,MNTable3D* ntable,int gid, int tag, ShapeList* sList)
{
  // get bounding box
  std::cout << "InsertGenerator3D::seedParticles" << std::endl;
  pair<Vector3,Vector3> bbx=vol->getBoundingBox();
  std::cout << "bbx: " << bbx.first << " - " << bbx.second << std::endl;
  double dx=(bbx.second.X()-bbx.first.X());
  double dy=(bbx.second.Y()-bbx.first.Y());
  double dz=(bbx.second.Z()-bbx.first.Z());
  // get index limits for seeding
  int imax=int(ceil(dx/(m_rmax*2.0)));
  int jmax=int(ceil(dy/(m_rmax*sqrt(3.0))));
  int kmax=int(ceil(dz/(m_rmax*2.0*sqrt(2.0/3.0))));
  // seed positions
  for(int i=0;i<=imax;i++){
    for(int j=0;j<=jmax;j++){
      for(int k=0;k<=kmax;k++){
	// get position
	// double px=bbx.first.X()+(double(i)+0.5*double(j%2)-0.25)*m_rmax*2.0;
// 	double py=bbx.first.Y()+(double(j)+(double(k%2)-0.5)/3.0)*sqrt(3.0)*m_rmax;
// 	double pz=bbx.first.Z()+(double(k)*2.0*sqrt(2.0/3.0))*m_rmax;
	double px=bbx.first.X()+((double(i)+0.5*double(j%2)+0.5*double(k%2))*m_rmax*2.0)+m_rmax+1e-5;
	double py=bbx.first.Y()+((double(j)+double(k%2)/3.0)*sqrt(3.0)*m_rmax)+m_rmax+1e-5;
	double pz=bbx.first.Z()+((double(k)*2.0*sqrt(2.0/3.0))*m_rmax)+m_rmax+1e-5;

	// get dist to egde
	double dex=(bbx.second.X()-px) < (px-bbx.first.X()) ? bbx.second.X()-px  : px-bbx.first.X();
	double dey=(bbx.second.Y()-py) < (py-bbx.first.Y()) ? bbx.second.Y()-py  : py-bbx.first.Y();
	double dez=(bbx.second.Z()-pz) < (pz-bbx.first.Z()) ? bbx.second.Z()-pz  : pz-bbx.first.Z();
	
	double de=(dex<dey) ? dex : dey;
	de = (de < dez) ? de : dez;

	// check max rad.
	if(de>m_rmin){
	  // calc random radius
	  double r;
	  if(de<m_rmax) {
	    r=m_rmin+((de-m_rmin)*((double)(rand())/(double)(RAND_MAX)));
	  } else {
	    r=m_rmin+((m_rmax-m_rmin)*((double)(rand())/(double)(RAND_MAX)));
	  }
	  //std::cout << "pos: " << Vector3(px,py,pz) << " r: " << r << std::endl;
	  Sphere S(Vector3(px,py,pz),r);
	  bool fit=vol->isIn(S) && ntable->checkInsertable(S,gid);
	  if(fit){
            // A sphere fits, so insert a "shape"
	    //S.setTag(tag);
            sList->insertShape(Vector3(px,py,pz),r,ntable,tag,gid);
	    //ntable->insertChecked(S,gid);
	    //std::cout << "inserted" << std::endl;
	  } // else {
// 	    std::cout << "isIn " << vol->isIn(S) << " insertable: " << ntable->checkInsertable(S,gid) << std::endl;
// 	  }
	}
      }
    }
  }

}


/*!
  Fill in particles

  \param vol a pointer to the packing volume
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
*/
void InsertGenerator3D::fillIn(AVolume3D* vol,MNTable3D* ntable,int gid, int tag)
{
  Sphere nsph;

  int total_tries=0;
  int count_insert=0;

  int nvol=vol->getNumberSubVolumes();
  //  for(int ivol=0;ivol<nvol;ivol++){
    int countfail=0; // number of failed attempts since last successfull insertion
    int last_fail_count=0; // number of failed attempts since last 100
    while(countfail<m_max_tries){
      bool findfit=false;
      Vector3 P=vol->getAPoint(0); // get random point within volume
      multimap<double,const Sphere*> close_particles=ntable->getSpheresClosestTo(P,4); // get 3 nearest spheres
      map<double,const AGeometricObject*> close_lines=vol->getClosestObjects(P,4); // get 2 nearest planes

      map<double,const AGeometricObject*> geomap;
      geomap.insert(close_particles.begin(),close_particles.end());
      geomap.insert(close_lines.begin(),close_lines.end());     
      
      if(geomap.size()>=4){
	map<double,const AGeometricObject*>::iterator iter=geomap.begin();
	const AGeometricObject* GO1=iter->second;iter++;
	const AGeometricObject* GO2=iter->second;iter++;
	const AGeometricObject* GO3=iter->second;iter++;
	const AGeometricObject* GO4=iter->second;
	nsph=FitSphere3D(GO1,GO2,GO3,GO4,P,m_max_iter,m_prec);
	findfit=true;
      }  

      if(findfit){
	// check if within radius range
	bool is_radius=(m_rmin<nsph.Radius()) && (m_rmax>nsph.Radius());
	
	// check inside volume, intersections ...
	bool fit=vol->isIn(nsph); // && ntable->checkInsertable(nsph,gid);
	
	if(fit && is_radius){ // acceptable particle 
	  nsph.setTag(tag);
	  if(ntable->insertChecked(nsph,gid)){; // insert
	    count_insert++;
	    total_tries+=countfail;
	    last_fail_count+=countfail;
	    if((count_insert%100)==0) {
	      std::cout << "inserted " << count_insert << " at avg. " << double(last_fail_count)*0.01 << std::endl;
	      last_fail_count=0;
	    }
	    countfail=0; // reset failure counter
	  } else countfail++;
	} else {
	  countfail++;
	} 
      } else countfail++; 
    }
    //  }
  std::cout << "total tries: " << total_tries << std::endl;
}

/*!
  Fill in particles

  \param vol a pointer to the packing volume
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
  \param tag the tag of the new particles
  \param maxvol maximum particle volume
*/
void InsertGenerator3D::fillIn(AVolume3D* vol,MNTable3D* ntable,int gid, int tag, double maxvol)
{

  // get vol. of particles already in ntable
  double tvol=ntable->getSumVolume(gid);
  std::cout << "particle volume: " << tvol << std::endl;
 
  Sphere nsph;

  int total_tries=0;
  int count_insert=0;

  int nvol=vol->getNumberSubVolumes();
  for(int ivol=0;ivol<nvol;ivol++){
    int countfail=0; // number of failed attempts since last successfull insertion
    int last_fail_count=0; // number of failed attempts since last 100
    while((countfail<m_max_tries) && (tvol<maxvol)){
      bool findfit=false;
      Vector3 P=vol->getAPoint(ivol); // get random point within volume
      multimap<double,const Sphere*> close_particles=ntable->getSpheresClosestTo(P,4); // get 3 nearest spheres
      map<double,const AGeometricObject*> close_lines=vol->getClosestObjects(P,3); // get 2 nearest planes

      map<double,const AGeometricObject*> geomap;
      geomap.insert(close_particles.begin(),close_particles.end());
      geomap.insert(close_lines.begin(),close_lines.end());     
      
      if(geomap.size()>=4){
	map<double,const AGeometricObject*>::iterator iter=geomap.begin();
	const AGeometricObject* GO1=iter->second;iter++;
	const AGeometricObject* GO2=iter->second;iter++;
	const AGeometricObject* GO3=iter->second;iter++;
	const AGeometricObject* GO4=iter->second;
	nsph=FitSphere3D(GO1,GO2,GO3,GO4,P,m_max_iter,m_prec);
	findfit=true;
      }  

      if(findfit){
	// check if within radius range
	bool is_radius=(m_rmin<nsph.Radius()) && (m_rmax>nsph.Radius());
	
	// check inside volume, intersections ...
	bool fit=vol->isIn(nsph) && ntable->checkInsertable(nsph,gid);
	
	if(fit && is_radius){ // acceptable particle 
	  nsph.setTag(tag);
	  ntable->insertChecked(nsph,gid); // insert
	  count_insert++;
	  double rad=nsph.Radius();
	  tvol+=(4.0/3.0)*3.1415926*rad*rad*rad; // V=4/3*pi*r^3
	  total_tries+=countfail;
	  last_fail_count+=countfail;
	  if((count_insert%100)==0) {
	    std::cout << "inserted " << count_insert << " with volume " << tvol << " at avg. " << double(last_fail_count)*0.01 << std::endl;
	    last_fail_count=0;
	  }
	  //if(countfail>m_max_tries/10) std::cout << countfail << " tries" << std::endl;
	  countfail=0; // reset failure counter
	} else {
	  countfail++;
	} 
      } else countfail++; 
    }
  }
  std::cout << "total tries: " << total_tries << std::endl;
}



/*!
  Fill in particles

  \param vol a pointer to the packing volume
  \param ntable a pointer to the neighbour table used 
  \param gid particle group id
*/
void InsertGenerator3D::fillIn(AVolume3D* vol,MNTable3D* ntable,int gid, int tag, ShapeList *list)
{
  Sphere nsph;

  int total_tries=0;
  int count_insert=0;

  int nvol=vol->getNumberSubVolumes();
  for(int ivol=0;ivol<nvol;ivol++){
    int countfail=0; // number of failed attempts since last successfull insertion
    int last_fail_count=0; // number of failed attempts since last 100
    while(countfail<m_max_tries){
      bool findfit=false;
      Vector3 P=vol->getAPoint(ivol); // get random point within volume
      multimap<double,const Sphere*> close_particles=ntable->getSpheresClosestTo(P,4); // get 3 nearest spheres
      map<double,const AGeometricObject*> close_lines=vol->getClosestObjects(P,3); // get 2 nearest planes

      map<double,const AGeometricObject*> geomap;
      geomap.insert(close_particles.begin(),close_particles.end());
      geomap.insert(close_lines.begin(),close_lines.end());     
      
      if(geomap.size()>=4){
	map<double,const AGeometricObject*>::iterator iter=geomap.begin();
	const AGeometricObject* GO1=iter->second;iter++;
	const AGeometricObject* GO2=iter->second;iter++;
	const AGeometricObject* GO3=iter->second;iter++;
	const AGeometricObject* GO4=iter->second;
	nsph=FitSphere3D(GO1,GO2,GO3,GO4,P,m_max_iter,m_prec);
	findfit=true;
      }  

      if(findfit){
	// check if within radius range
	bool is_radius=(m_rmin<nsph.Radius()) && (m_rmax>nsph.Radius());
	
	// check inside volume, intersections ...
	bool fit=vol->isIn(nsph) && ntable->checkInsertable(nsph,gid);
	
	if(fit && is_radius){ // acceptable particle 
	  nsph.setTag(tag);
//	  ntable->insertChecked(nsph,gid); // insert
	  list->insertShape(nsph.Center(),nsph.Radius(),ntable,tag,gid);
	  count_insert++;
	  total_tries+=countfail;
	  last_fail_count+=countfail;
	  if((count_insert%100)==0) {
	    std::cout << "inserted " << count_insert << " at avg. " << double(last_fail_count)*0.01 << std::endl;
	    last_fail_count=0;
	  }
	  //if(countfail>m_max_tries/10) std::cout << countfail << " tries" << std::endl;
	  countfail=0; // reset failure counter
	} else {
	  countfail++;
	} 
      } else countfail++; 
    }
  }
  std::cout << "total tries: " << total_tries << std::endl;
}

ostream& operator << (ostream& ost,const InsertGenerator3D& T)
{
   return ost;
}


