/////////////////////////////////////////////////////////////
//                                                         //
// 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 "HexAggregateInsertGenerator3D.h"
// --- System includes ---

#include <cmath>
#include <cstdlib>

using std::ceil;
using std::sqrt;
using std::sin;
using std::cos;

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


HexAggregateInsertGenerator3D::HexAggregateInsertGenerator3D()
{}

/*!
  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
*/
HexAggregateInsertGenerator3D::HexAggregateInsertGenerator3D(double rmin,double rmax ,int ntries,int max_iter,double prec, bool seed)
  :InsertGenerator3D(rmin,rmax,ntries,max_iter,prec,seed) 
{}

/*!
  take a sphere and insert a aggregate grain into the ntable instead

  \param S the sphere
*/
void HexAggregateInsertGenerator3D::ParticleToAggregate(MNTable3D* ntable, const Sphere& S, int gid)
{
  double rn=S.Radius()/3.0; // new radii
  // center sphere
  Sphere Sc(S.Center(),rn);
  Sc.setTag(S.Tag());
  ntable->insertChecked(Sc,gid);
  int Sc_id=Sc.Id();
  // outer spheres
  int Sk_id[6];
  for(int k=0;k<6;k++){
    double phi=double(k)*1.04719551; // k*pi/3
    Vector3 offset=Vector3(2.0000*rn*sin(phi),2.0000*rn*cos(phi),0.0);
    Sphere Sk(S.Center()+offset,rn*0.99999);
    if(ntable->checkInsertable(Sk,gid)){
      Sk.setTag(S.Tag());
      ntable->insert(Sk,gid);
      Sk_id[k]=Sk.Id();
      ntable->insertBond(Sc_id,Sk_id[k],0); // bond between center and outer
    } else {
      Sk_id[k]=-1;
    }
  }
  for(int k=0;k<6;k++){
    int k2=(k+1) % 6;
    if((Sk_id[k]!=-1) && (Sk_id[k2]!=-1)) {
      ntable->insertBond(Sk_id[k],Sk_id[k2],0);
    }
  }
  // upper spheres
  int Sk_up[3];
  double alpha=0.5235987755982988; // pi/6 (30°)
  double beta=1.5707963267948965-atan(0.7071067811865475);
  for(int k=0;k<3;k++){
    double rho=beta;
    double phi=double(1+4*k)*alpha;
    Vector3 offset=Vector3(2.0*rn*sin(phi)*cos(rho),
		     2.0*rn*cos(phi)*cos(rho),
		     2.0*rn*sin(rho));
    Sphere Sk(S.Center()+offset,rn*0.99999);
    if(ntable->checkInsertable(Sk,gid)){
      Sk.setTag(S.Tag());
      ntable->insert(Sk,gid);
      Sk_up[k]=Sk.Id();
      ntable->insertBond(Sc_id,Sk_up[k],0); // bond between center and upper
      if(Sk_id[k*2]!=-1) ntable->insertBond(Sk_id[k*2],Sk_up[k],0);
      if(Sk_id[(k*2+1)%6]!=-1) ntable->insertBond(Sk_id[(k*2+1)%6],Sk_up[k],0);
    } else {
      Sk_up[k]=-1;
    }
  }
  // bond within upper
  for(int k=0;k<3;k++){
    int k2=(k+1) % 3;
    if((Sk_up[k]!=-1) && (Sk_up[k2]!=-1)) {
      ntable->insertBond(Sk_up[k],Sk_up[k2],0);
    }
  }
  // lower spheres
  for(int k=0;k<3;k++){
    double rho=beta;
    double phi=double(1+4*k)*alpha;
    Vector3 offset=Vector3(2.0*rn*sin(phi)*cos(rho),
		     2.0*rn*cos(phi)*cos(rho),
		     -2.0*rn*sin(rho));
    Sphere Sk(S.Center()+offset,rn*0.99999);
    if(ntable->checkInsertable(Sk,gid)){
      Sk.setTag(S.Tag());
      ntable->insert(Sk,gid);
      Sk_up[k]=Sk.Id();
      ntable->insertBond(Sc_id,Sk_up[k],0); // bond between center and upper
      if(Sk_id[k*2]!=-1) ntable->insertBond(Sk_id[k*2],Sk_up[k],0);
      if(Sk_id[(k*2+1)%6]!=-1) ntable->insertBond(Sk_id[(k*2+1)%6],Sk_up[k],0);
    } else {
      Sk_up[k]=-1;
    }
  }
  // bond within upper
  for(int k=0;k<3;k++){
    int k2=(k+1) % 3;
    if((Sk_up[k]!=-1) && (Sk_up[k2]!=-1)) {
      ntable->insertBond(Sk_up[k],Sk_up[k2],0);
    }
  }
}

/*!
  seed the volume 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 particle tag
*/
void HexAggregateInsertGenerator3D::seedParticles(AVolume3D* vol,MNTable3D* ntable,int gid,int tag)
{  
  std::cout << "HexAggregateInsertGenerator3D::seedParticles" << std::endl;
  // get bounding box
  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++){
	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)));
	  }
	  Sphere S(Vector3(px,py,pz),r);
	  S.setTag(tag);
	  bool fit=vol->isIn(S) && ntable->checkInsertable(S,gid);
	  if(fit){
	    ParticleToAggregate(ntable,S,gid);
	  } 
	}
      }
    }
  }
}

/*!
  fill the remaining volume 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 particle tag
*/
void HexAggregateInsertGenerator3D::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(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);
	  ParticleToAggregate(ntable,nsph,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;
}
