// Gmsh - Copyright (C) 1997-2015 C. Geuzaine, J.-F. Remacle
//
// See the LICENSE.txt file for license information. Please report all
// bugs and problems to the public mailing list <gmsh@geuz.org>.
//
// Contributor(s):
//   Tristan Carrier François Henrotte


#include "simple3D.h"
#include "GModel.h"
#include "MElement.h"
#include "MElementOctree.h"
#include "meshGRegion.h"
#include <queue>
#include <fstream>
#include "CenterlineField.h"
#include <algorithm>
#include "directions3D.h"
#include "Context.h"
#include <iostream>
#include <string>
#include "rtree.h"

#define k1 0.7 //k1*h is the minimal distance between two nodes
#define k2 0.5 //k2*h is the minimal distance to the boundary
#define sqrt3 1.73205081

/*********definitions*********/

class Metric{
 private:
  double m11,m21,m31,m12,m22,m32,m13,m23,m33;
 public:
  Metric();
  ~Metric();
  void set_m11(double);
  void set_m21(double);
  void set_m31(double);
  void set_m12(double);
  void set_m22(double);
  void set_m32(double);
  void set_m13(double);
  void set_m23(double);
  void set_m33(double);
  double get_m11();
  double get_m21();
  double get_m31();
  double get_m12();
  double get_m22();
  double get_m32();
  double get_m13();
  double get_m23();
  double get_m33();
};

class Node{
 private:
  int layer;
  int limit;
  double h;
  Metric m;
  SPoint3 point;
 public:
  double min[3];
  double max[3];
  Node();
  Node(SPoint3);
  ~Node();
  void set_layer(int);
  void set_limit(int);
  void set_size(double);
  void set_metric(Metric);
  void set_point(SPoint3);
  int get_layer();
  int get_limit();
  double get_size();
  Metric get_metric();
  SPoint3 get_point();
};

class Wrapper{
 private:
  bool ok;
  Node* individual;
  Node* parent;
 public:
  Wrapper();
  Wrapper(Node*,Node*);
  ~Wrapper();
  void set_ok(bool);
  void set_individual(Node*);
  void set_parent(Node*);
  bool get_ok();
  Node* get_individual();
  Node* get_parent();
};

/*********functions*********/

double infinity_distance(SPoint3 p1,SPoint3 p2,Metric m)
{
  double distance;
  double x1,y1,z1;
  double x2,y2,z2;
  double a,b,c,d,e,f,g,h,i;

  a = m.get_m11();
  b = m.get_m21();
  c = m.get_m31();
  d = m.get_m12();
  e = m.get_m22();
  f = m.get_m32();
  g = m.get_m13();
  h = m.get_m23();
  i = m.get_m33();

  x1 = a*p1.x() + b*p1.y() + c*p1.z();
  y1 = d*p1.x() + e*p1.y() + f*p1.z();
  z1 = g*p1.x() + h*p1.y() + i*p1.z();

  x2 = a*p2.x() + b*p2.y() + c*p2.z();
  y2 = d*p2.x() + e*p2.y() + f*p2.z();
  z2 = g*p2.x() + h*p2.y() + i*p2.z();

  distance = std::max(std::max(fabs(x2-x1),fabs(y2-y1)),fabs(z2-z1));
  return distance;
}

bool rtree_callback(Node* neighbour,void* w)
{
  double h;
  double distance;
  Metric m;
  Node *individual,*parent;
  Wrapper* wrapper;

  wrapper = static_cast<Wrapper*>(w);
  individual = wrapper->get_individual();
  parent = wrapper->get_parent();
  h = individual->get_size();
  m = individual->get_metric();

  if(neighbour!=parent){
    distance = infinity_distance(individual->get_point(),neighbour->get_point(),m);
	if(distance<k1*h){
	  wrapper->set_ok(0);
	  return false;
	}
  }

  return true;
}

/*********class Metric*********/

Metric::Metric()
{
  m11 = 1.0;
  m21 = 0.0;
  m31 = 0.0;
  m12 = 0.0;
  m22 = 1.0;
  m32 = 0.0;
  m13 = 0.0;
  m23 = 0.0;
  m33 = 1.0;
}

Metric::~Metric(){}

void Metric::set_m11(double new_m11){ m11 = new_m11; }

void Metric::set_m21(double new_m21){ m21 = new_m21; }

void Metric::set_m31(double new_m31){ m31 = new_m31; }

void Metric::set_m12(double new_m12){ m12 = new_m12; }

void Metric::set_m22(double new_m22){ m22 = new_m22; }

void Metric::set_m32(double new_m32){ m32 = new_m32; }

void Metric::set_m13(double new_m13){ m13 = new_m13; }

void Metric::set_m23(double new_m23){ m23 = new_m23; }

void Metric::set_m33(double new_m33){ m33 = new_m33; }

double Metric::get_m11(){ return m11; }

double Metric::get_m21(){ return m21; }

double Metric::get_m31(){ return m31; }

double Metric::get_m12(){ return m12; }

double Metric::get_m22(){ return m22; }

double Metric::get_m32(){ return m32; }

double Metric::get_m13(){ return m13; }

double Metric::get_m23(){ return m23; }

double Metric::get_m33(){ return m33; }

/*********class Node*********/

Node::Node(){
  limit = -1;
}

Node::Node(SPoint3 new_point){
  point = new_point;
  limit = -1;
}

Node::~Node(){}

void Node::set_layer(int new_layer){
  layer = new_layer;
}

void Node::set_limit(int new_limit){
  limit = new_limit;
}

void Node::set_size(double new_h){
  h = new_h;
}

void Node::set_metric(Metric new_m){
  m = new_m;
}

void Node::set_point(SPoint3 new_point){
  point = new_point;
}

int Node::get_layer(){
  return layer;
}

int Node::get_limit(){
  return limit;
}

double Node::get_size(){
  return h;
}

Metric Node::get_metric(){
  return m;
}

SPoint3 Node::get_point(){
  return point;
}

/*********class Wrapper*********/

Wrapper::Wrapper(){
  ok = 1;
}

Wrapper::Wrapper(Node* new_individual,Node* new_parent){
  ok = 1;
  individual = new_individual;
  parent = new_parent;
}

Wrapper::~Wrapper(){}

void Wrapper::set_ok(bool new_ok){
  ok = new_ok;
}

void Wrapper::set_individual(Node* new_individual){
  individual = new_individual;
}

void Wrapper::set_parent(Node* new_parent){
  parent = new_parent;
}

bool Wrapper::get_ok(){
  return ok;
}

Node* Wrapper::get_individual(){
  return individual;
}

Node* Wrapper::get_parent(){
  return parent;
}

/*********class Filler*********/

Filler::Filler(){}

Filler::~Filler(){}

void Filler::treat_model()
{
  GRegion* gr;
  GModel* model = GModel::current();
  GModel::riter it;

  for(it=model->firstRegion();it!=model->lastRegion();it++)
  {
    gr = *it;
	if(gr->getNumMeshElements()>0){
	  treat_region(gr);
	}
  }
}

void Filler::treat_region(GRegion* gr)
{
  int NumSmooth = CTX::instance()->mesh.smoothCrossField;
  std::cout << "NumSmooth = " << NumSmooth << std::endl ;
  if(NumSmooth && (gr->dim() == 3)){
    double scale = gr->bounds().diag()*1e-2;
    Frame_field::initRegion(gr,NumSmooth);
    Frame_field::saveCrossField("cross0.pos",scale);

    Frame_field::smoothRegion(gr,NumSmooth);
    Frame_field::saveCrossField("cross1.pos",scale);
  }

  unsigned int i;
  int j;
  int count;
  int limit;
  bool ok2;
  double x,y,z;
  SPoint3 point;
  Node *node,*individual,*parent;
  MVertex* vertex;
  MElement* element;
  MElementOctree* octree;
  deMeshGRegion deleter;
  Wrapper wrapper;
  GFace* gf;
  std::queue<Node*> fifo;
  std::vector<Node*> spawns;
  std::vector<Node*> garbage;
  std::vector<MVertex*> boundary_vertices;
  std::set<MVertex*> temp;
  std::list<GFace*> faces;
  std::map<MVertex*,int> limits;
  std::set<MVertex*>::iterator it;
  std::list<GFace*>::iterator it2;
  std::map<MVertex*,int>::iterator it3;
  RTree<Node*,double,3,double> rtree;

  Frame_field::init_region(gr);
  Size_field::init_region(gr);
  Size_field::solve(gr);
  octree = new MElementOctree(gr->model());
  garbage.clear();
  boundary_vertices.clear();
  temp.clear();
  new_vertices.clear();
  faces.clear();
  limits.clear();

  faces = gr->faces();
  for(it2=faces.begin();it2!=faces.end();it2++){
    gf = *it2;
	limit = code(gf->tag());
	for(i=0;i<gf->getNumMeshElements();i++){
	  element = gf->getMeshElement(i);
      for(j=0;j<element->getNumVertices();j++){
	    vertex = element->getVertex(j);
		temp.insert(vertex);
		limits.insert(std::pair<MVertex*,int>(vertex,limit));
	  }
	}
  }

  /*for(i=0;i<gr->getNumMeshElements();i++){
    element = gr->getMeshElement(i);
    for(j=0;j<element->getNumVertices();j++){
      vertex = element->getVertex(j);
      temp.insert(vertex);
    }
  }*/

  for(it=temp.begin();it!=temp.end();it++){
    if((*it)->onWhat()->dim()==0){
	  boundary_vertices.push_back(*it);
	}
  }

  for(it=temp.begin();it!=temp.end();it++){
    if((*it)->onWhat()->dim()==1){
	  boundary_vertices.push_back(*it);
	}
  }

  for(it=temp.begin();it!=temp.end();it++){
    if((*it)->onWhat()->dim()==2){
	  boundary_vertices.push_back(*it);
	}
  }

  /*for(it=temp.begin();it!=temp.end();it++){
    if((*it)->onWhat()->dim()<3){
      boundary_vertices.push_back(*it);
    }
  }*/
  //std::ofstream file("nodes.pos");
  //file << "View \"test\" {\n";

  for(i=0;i<boundary_vertices.size();i++){
    x = boundary_vertices[i]->x();
    y = boundary_vertices[i]->y();
    z = boundary_vertices[i]->z();

    node = new Node(SPoint3(x,y,z));
    compute_parameters(node,gr);
	node->set_layer(0);

	it3 = limits.find(boundary_vertices[i]);
	node->set_limit(it3->second);

	rtree.Insert(node->min,node->max,node);
	fifo.push(node);
    //print_node(node,file);
  }

  count = 1;
  while(!fifo.empty()){
    parent = fifo.front();
	fifo.pop();
	garbage.push_back(parent);

	if(parent->get_limit()!=-1 && parent->get_layer()>=parent->get_limit()){
	  continue;
	}

	spawns.clear();
	spawns.resize(6);

	for(i=0;i<6;i++){
	  spawns[i] = new Node();
	}

	create_spawns(gr,octree,parent,spawns);

	for(i=0;i<6;i++){
	  ok2 = 0;
	  individual = spawns[i];
	  point = individual->get_point();
	  x = point.x();
	  y = point.y();
	  z = point.z();

	  if(inside_domain(octree,x,y,z)){
		compute_parameters(individual,gr);
		individual->set_layer(parent->get_layer()+1);
		individual->set_limit(parent->get_limit());

		if(far_from_boundary(octree,individual)){
		  wrapper.set_ok(1);
		  wrapper.set_individual(individual);
		  wrapper.set_parent(parent);
		  rtree.Search(individual->min,individual->max,rtree_callback,&wrapper);

		  if(wrapper.get_ok()){
		    fifo.push(individual);
		    rtree.Insert(individual->min,individual->max,individual);
			vertex = new MVertex(x,y,z,gr,0);
			new_vertices.push_back(vertex);
			ok2 = 1;
			//print_segment(individual->get_point(),parent->get_point(),file);
		  }
	    }
	  }

	  if(!ok2) delete individual;
	}

	if(count%100==0){
	  printf("%d\n",count);
	}
	count++;
  }

  //file << "};\n";

  int option = CTX::instance()->mesh.algo3d;
  CTX::instance()->mesh.algo3d = ALGO_3D_DELAUNAY;

  deleter(gr);
  std::vector<GRegion*> regions;
  regions.push_back(gr);
  meshGRegion mesher(regions); //?
  mesher(gr); //?
  MeshDelaunayVolume(regions);

  CTX::instance()->mesh.algo3d = option;

  for(i=0;i<garbage.size();i++) delete garbage[i];
  for(i=0;i<new_vertices.size();i++) delete new_vertices[i];
  new_vertices.clear();
  delete octree;
  rtree.RemoveAll();
  Size_field::clear();
  Frame_field::clear();
}

Metric Filler::get_metric(double x,double y,double z)
{
  Metric m;
  STensor3 m2;
  if(CTX::instance()->mesh.smoothCrossField){
    m2 = Frame_field::findCross(x,y,z);
  }
  else
    m2 = Frame_field::search(x,y,z);

  m.set_m11(m2.get_m11());
  m.set_m21(m2.get_m21());
  m.set_m31(m2.get_m31());

  m.set_m12(m2.get_m12());
  m.set_m22(m2.get_m22());
  m.set_m32(m2.get_m32());

  m.set_m13(m2.get_m13());
  m.set_m23(m2.get_m23());
  m.set_m33(m2.get_m33());

  return m;
}

Metric Filler::get_metric(double x,double y,double z,GEntity* ge)
{
  Metric m;
  SMetric3 temp;
  SVector3 v1,v2,v3;
  Field* field;
  FieldManager* manager;

  v1 = SVector3(1.0,0.0,0.0);
  v2 = SVector3(0.0,1.0,0.0);
  v3 = SVector3(0.0,0.0,1.0);

  manager = ge->model()->getFields();
  if(manager->getBackgroundField()>0){
    field = manager->get(manager->getBackgroundField());
    if(field){
      (*field)(x,y,z,temp,ge);
    }
  }

  m.set_m11(v1.x());
  m.set_m21(v1.y());
  m.set_m31(v1.z());

  m.set_m12(v2.x());
  m.set_m22(v2.y());
  m.set_m32(v2.z());

  m.set_m13(v3.x());
  m.set_m23(v3.y());
  m.set_m33(v3.z());

  return m;
}

double Filler::get_size(double x,double y,double z){

  return Size_field::search(x,y,z);
}

double Filler::get_size(double x,double y,double z,GEntity* ge)
{
  double h;
  Field* field;
  FieldManager* manager;

  h = 1.0;
  manager = ge->model()->getFields();
  if(manager->getBackgroundField()>0){
    field = manager->get(manager->getBackgroundField());
	if(field){
	  h = (*field)(x,y,z,ge);
	}
  }

  return h;
}

bool Filler::inside_domain(MElementOctree* octree,double x,double y,double z)
{
  MElement* element;
  element = (MElement*)octree->find(x,y,z,3,true);
  if(element!=NULL) return 1;
  else return 0;
}

bool Filler::far_from_boundary(MElementOctree* octree,Node* node)
{
  double x,y,z;
  double h;
  SPoint3 point;
  MElement *e1,*e2,*e3,*e4,*e5,*e6;

  point = node->get_point();
  x = point.x();
  y = point.y();
  z = point.z();
  h = node->get_size();

  e1 = (MElement*)octree->find(x+k2*h,y,z,3,true);
  e2 = (MElement*)octree->find(x-k2*h,y,z,3,true);
  e3 = (MElement*)octree->find(x,y+k2*h,z,3,true);
  e4 = (MElement*)octree->find(x,y-k2*h,z,3,true);
  e5 = (MElement*)octree->find(x,y,z+k2*h,3,true);
  e6 = (MElement*)octree->find(x,y,z-k2*h,3,true);

  if(e1!=NULL && e2!=NULL && e3!=NULL && e4!=NULL && e5!=NULL && e6!=NULL) return 1;
  else return 0;
}

void Filler::compute_parameters(Node* node,GEntity* ge)
{
  double x,y,z;
  double h;
  Metric m;
  SPoint3 point;

  point = node->get_point();
  x = point.x();
  y = point.y();
  z = point.z();
  m = get_metric(x,y,z);
  h = get_size(x,y,z);

  node->set_size(h);
  node->set_metric(m);
  node->min[0] = x - sqrt3*h;
  node->min[1] = y - sqrt3*h;
  node->min[2] = z - sqrt3*h;
  node->max[0] = x + sqrt3*h;
  node->max[1] = y + sqrt3*h;
  node->max[2] = z + sqrt3*h;
}

void Filler::create_spawns(GEntity* ge,MElementOctree* octree,
                           Node* node,std::vector<Node*>& spawns)
{
  double x,y,z;
  double x1,y1,z1;
  double x2,y2,z2;
  double x3,y3,z3;
  double x4,y4,z4;
  double x5,y5,z5;
  double x6,y6,z6;
  double h;
  double h1,h2,h3,h4,h5,h6;
  Metric m;
  SPoint3 point;

  point = node->get_point();
  x = point.x();
  y = point.y();
  z = point.z();
  h = node->get_size();
  m = node->get_metric();

  h1 = improvement(ge,octree,point,h,SVector3(m.get_m11(),m.get_m21(),m.get_m31()));
  x1 = x + h1*m.get_m11();
  y1 = y + h1*m.get_m21();
  z1 = z + h1*m.get_m31();

  h2 = improvement(ge,octree,point,h,SVector3(-m.get_m11(),-m.get_m21(),-m.get_m31()));
  x2 = x - h2*m.get_m11();
  y2 = y - h2*m.get_m21();
  z2 = z - h2*m.get_m31();

  h3 = improvement(ge,octree,point,h,SVector3(m.get_m12(),m.get_m22(),m.get_m32()));
  x3 = x + h3*m.get_m12();
  y3 = y + h3*m.get_m22();
  z3 = z + h3*m.get_m32();

  h4 = improvement(ge,octree,point,h,SVector3(-m.get_m12(),-m.get_m22(),-m.get_m32()));
  x4 = x - h4*m.get_m12();
  y4 = y - h4*m.get_m22();
  z4 = z - h4*m.get_m32();

  h5 = improvement(ge,octree,point,h,SVector3(m.get_m13(),m.get_m23(),m.get_m33()));
  x5 = x + h5*m.get_m13();
  y5 = y + h5*m.get_m23();
  z5 = z + h5*m.get_m33();

  h6 = improvement(ge,octree,point,h,SVector3(-m.get_m13(),-m.get_m23(),-m.get_m33()));
  x6 = x - h6*m.get_m13();
  y6 = y - h6*m.get_m23();
  z6 = z - h6*m.get_m33();

  *spawns[0] = Node(SPoint3(x1,y1,z1));
  *spawns[1] = Node(SPoint3(x2,y2,z2));
  *spawns[2] = Node(SPoint3(x3,y3,z3));
  *spawns[3] = Node(SPoint3(x4,y4,z4));
  *spawns[4] = Node(SPoint3(x5,y5,z5));
  *spawns[5] = Node(SPoint3(x6,y6,z6));
}

double Filler::improvement(GEntity* ge,MElementOctree* octree,
                           SPoint3 point,double h1,SVector3 direction)
{
  double x,y,z;
  double average;
  double h2;
  double coeffA,coeffB;

  x = point.x() + h1*direction.x();
  y = point.y() + h1*direction.y();
  z = point.z() + h1*direction.z();

  if(inside_domain(octree,x,y,z)){
    h2 = get_size(x,y,z);
  }
  else h2 = h1;

  coeffA = 1.0;
  coeffB = 0.16;

  if(h2>h1){
    average = coeffA*h1 + (1.0-coeffA)*h2;
  }
  else{
    average = coeffB*h1 + (1.0-coeffB)*h2;
  }

  return average;
}

int Filler::code(int tag){
  int limit;
  std::string s;
  std::stringstream s2;

  limit = -1;
  s2 << tag;
  s = s2.str();

  if(s.length()>=5){
    if(s.at(0)=='1' && s.at(1)=='1' && s.at(2)=='1' && s.at(3)=='1' && s.at(4)=='1'){
	  limit = 0;
	}
	else if(s.at(0)=='2' && s.at(1)=='2' && s.at(2)=='2' && s.at(3)=='2' && s.at(4)=='2'){
	  limit = 1;
	}
  }

  return limit;
}

int Filler::get_nbr_new_vertices()
{
  return new_vertices.size();
}

MVertex* Filler::get_new_vertex(int i)
{
  return new_vertices[i];
}

void Filler::print_segment(SPoint3 p1,SPoint3 p2,std::ofstream& file)
{
  file << "SL ("
  << p1.x() << ", " << p1.y() << ", " << p1.z() << ", "
  << p2.x() << ", " << p2.y() << ", " << p2.z() << ")"
  << "{10, 20};\n";
}

void Filler::print_node(Node* node,std::ofstream& file)
{
  double x,y,z;
  double x1,y1,z1;
  double x2,y2,z2;
  double x3,y3,z3;
  double x4,y4,z4;
  double x5,y5,z5;
  double x6,y6,z6;
  double h;
  Metric m;
  SPoint3 point;

  point = node->get_point();
  x = point.x();
  y = point.y();
  z = point.z();
  h = node->get_size();
  m = node->get_metric();

  x1 = x + k1*h*m.get_m11();
  y1 = y + k1*h*m.get_m21();
  z1 = z + k1*h*m.get_m31();

  x2 = x - k1*h*m.get_m11();
  y2 = y - k1*h*m.get_m21();
  z2 = z - k1*h*m.get_m31();

  x3 = x + k1*h*m.get_m12();
  y3 = y + k1*h*m.get_m22();
  z3 = z + k1*h*m.get_m32();

  x4 = x - k1*h*m.get_m12();
  y4 = y - k1*h*m.get_m22();
  z4 = z - k1*h*m.get_m32();

  x5 = x + k1*h*m.get_m13();
  y5 = y + k1*h*m.get_m23();
  z5 = z + k1*h*m.get_m33();

  x6 = x - k1*h*m.get_m13();
  y6 = y - k1*h*m.get_m23();
  z6 = z - k1*h*m.get_m33();

  print_segment(SPoint3(x,y,z),SPoint3(x1,y1,z1),file);
  print_segment(SPoint3(x,y,z),SPoint3(x2,y2,z2),file);
  print_segment(SPoint3(x,y,z),SPoint3(x3,y3,z3),file);
  print_segment(SPoint3(x,y,z),SPoint3(x4,y4,z4),file);
  print_segment(SPoint3(x,y,z),SPoint3(x5,y5,z5),file);
  print_segment(SPoint3(x,y,z),SPoint3(x6,y6,z6),file);
}

/*********static declarations*********/

std::vector<MVertex*> Filler::new_vertices;

