#include "molecules.hpp"
#include "boundary.hpp"


void Find_next_suitable_point(atom a, atom b, atom Candidate, int n, Vector *d1, Vector *d2, double *Storage, const double RADIUS, molecule mol)
{
  /* d2 ist der Normalenvektor auf dem Dreieck,
   * d1 ist der Vektor, der normal auf der Kante und d2 steht.
   */
  Vector dif_a;
  Vector dif_b;
  Vector Chord;
  Vector AngleCheck;
  atom *Walker;

  dif_a.CopyVector(&a.x);
  dif_a.SubtractVector(&Candidate.x);
  dif_b.CopyVector(&b.x);
  dif_b.SubtractVector(&Candidate.x);
  Chord.CopyVector(&a.x);
  Chord.SubtractVector(&b.x);
  AngleCheck.CopyVector(&dif_a);
  AngleCheck.ProjectOntoPlane(&Chord);

  //Storage eintrag fuer aktuelles Atom
  if (Chord.Norm()/(2*sin(dif_a.Angle(&dif_b)))<RADIUS) //Using Formula for relation of chord length with inner angle to find of Ball will touch atom
  {

    if (dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1))>Storage[1]) //This will give absolute preference to those in "right-hand" quadrants
      {
	Storage[0]=(double)Candidate.nr;
	Storage[1]=1;
	Storage[2]=AngleCheck.Angle(d2);
      }
    else
      {
	if ((dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1)) == Storage[1] && Storage[1]>0 &&  Storage[2]< AngleCheck.Angle(d2)) or \
	    (dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1)) == Storage[1] && Storage[1]<0 &&  Storage[2]> AngleCheck.Angle(d2)))
	  //Depending on quadrant we prefer higher or lower atom with respect to Triangle normal first.
	  {
	    Storage[0]=(double)Candidate.nr;
	    Storage[1]=dif_a.ScalarProduct(d1)/fabs(dif_a.ScalarProduct(d1));
	    Storage[2]=AngleCheck.Angle(d2);
	  }
      }
  }

  if (n<5)
    {
      for(int i=0; i<mol.NumberOfBondsPerAtom[Candidate.nr];i++)
	{
	  while (Candidate.nr != (mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr ==Candidate.nr ? mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr : mol.ListOfBondsPerAtom[Candidate.nr][i]->rightatom->nr))
	    {
	      Walker = Walker->next;
	    }

	  Find_next_suitable_point(a, b, *Walker, n+1, d1, d2, Storage, RADIUS, mol);
	}
    }
};


void Tesselation::Find_next_suitable_triangle(molecule *mol, BoundaryLineSet Line, BoundaryTriangleSet T, const double& RADIUS)
{
  Vector CenterOfLine = Line.endpoints[0]->node->x;
  Vector direction1;
  Vector direction2;
  Vector helper;
  atom* Walker;

  double Storage[3];
  Storage[0]=-1.;   // Id must be positive, we see should nothing be done
  Storage[1]=-1.;   // This direction is either +1 or -1 one, so any result will take precedence over initial values
  Storage[2]=-10.;  // This is also lower then any value produced by an eligible atom, which are all positive

  
  helper.CopyVector(&(Line.endpoints[0]->node->x));
  for (int i =0; i<3; i++)
    {
      if (T.endpoints[i]->node->nr != Line.endpoints[0]->node->nr && T.endpoints[i]->node->nr!=Line.endpoints[1]->node->nr)
	{
	  helper.SubtractVector(&T.endpoints[i]->node->x);
	  break;
	}
    }


  direction1.CopyVector(&Line.endpoints[0]->node->x);
  direction1.SubtractVector(&Line.endpoints[1]->node->x);
  direction1.VectorProduct(T.NormalVector);

  if (direction1.ScalarProduct(&helper)>0)
    {
      direction1.Scale(-1);
    }

  Find_next_suitable_point(*Line.endpoints[0]->node, *Line.endpoints[1]->node, *Line.endpoints[0]->node, 0, &direction1, T.NormalVector, Storage, RADIUS, *mol);
  
  // Konstruiere nun neues Dreieck am Ende der Liste der Dreiecke
  // Next Triangle is Line, atom with number in Storage[0]

  Walker= mol->start;
  while (Walker->nr != (int)Storage[0])
    {
      Walker = Walker->next;
    }

  AddPoint(Walker);

  BPS[0] = new class BoundaryPointSet(Walker);
  BPS[1] = new class BoundaryPointSet(Line.endpoints[0]->node);
  BLS[0] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BPS[0] = new class BoundaryPointSet(Walker);
  BPS[1] = new class BoundaryPointSet(Line.endpoints[1]->node);
  BLS[1] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BLS[2] = new class BoundaryLineSet(Line);

  BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
  TrianglesOnBoundary.insert( TrianglePair(TrianglesOnBoundaryCount, BTS) );
  TrianglesOnBoundaryCount++;

  for(int i=0;i<NDIM;i++) // sind Linien bereits vorhanden ???
    {
      if (LinesOnBoundary.find(BTS->lines[i]) == LinesOnBoundary.end)
	{
	  LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BTS->lines[i]) );
	  LinesOnBoundaryCount++;
	}
    }
  BTS->GetNormalVector(*BTS->NormalVector);

  if( (BTS->NormalVector->ScalarProduct(T.NormalVector)<0 && Storage[1]>0) || \
      (BTS->NormalVector->ScalarProduct(T.NormalVector)>0 && Storage[1]<0))
    {
      BTS->NormalVector->Scale(-1);
    }
  
};


void Find_second_point_for_Tesselation(atom a, atom Candidate, int n, Vector Oben, double* Storage, molecule mol)
{
  int i;
  Vector *AngleCheck;
  atom* Walker;

  AngleCheck->CopyVector(&Candidate.x);
  AngleCheck->SubtractVector(&a.x);
  if (AngleCheck->ScalarProduct(&Oben) < Storage[1])
    {
      Storage[0]=(double)(Candidate.nr);
      Storage[1]=AngleCheck->ScalarProduct(&Oben);
    };

  if (n<5)
    {
      for (i = 0; i< mol.NumberOfBondsPerAtom[Candidate.nr]; i++)
	{
	  Walker = mol.start;
	  while (Candidate.nr != (mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr ==Candidate.nr ? mol.ListOfBondsPerAtom[Candidate.nr][i]->leftatom->nr : mol.ListOfBondsPerAtom[Candidate.nr][i]->rightatom->nr))
	    {
	      Walker = Walker->next;
	    };
	  
	  Find_second_point_for_Tesselation(a, *Walker, n+1, Oben, Storage, mol);
	    };
    };
  

};


void Tesselation::Find_starting_triangle(molecule mol, const double RADIUS)
{
  int i=0;
  atom Walker;
  atom Walker2;
  atom Walker3;
  int max_index[3];
  double max_coordinate[3];
  Vector Oben;
  Vector helper;

  Oben.Zero();


  for(i =0; i<3; i++)
    {
      max_index[i] =-1;
      max_coordinate[i] =-1;
    }

  Walker = *mol.start;
  while (Walker.next != NULL)
    {
      for (i=0; i<3; i++)
	{
	  if (Walker.x.x[i]>max_coordinate[i])
	    {
	      max_coordinate[i]=Walker.x.x[i];
	      max_index[i]=Walker.nr;
	    }
	}
    }

  //Koennen dies fuer alle Richtungen, legen hier erstmal Richtung auf k=0
  const int k=0;

  Oben.x[k]=1;
  Walker = *mol.start;
  while (Walker.nr != max_index[k])
    {
      Walker = *Walker.next;
    }

  double Storage[3];
  Storage[0]=-1.;   // Id must be positive, we see should nothing be done
  Storage[1]=-2.;   // This will contain the angle, which will be always positive (when looking for second point), when looking for third point this will be the quadrant.
  Storage[2]=-10.;  // This will be an angle looking for the third point.


  for (i=0; i< mol.NumberOfBondsPerAtom[Walker.nr]; i++)
    {
      Walker2 = *mol.start;
      while (Walker2.nr != (mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom->nr == Walker.nr ? mol.ListOfBondsPerAtom[Walker.nr][i]->rightatom->nr : mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom->nr) ) 
	{
	  Walker2 = *Walker2.next;
	}

      Find_second_point_for_Tesselation(Walker, Walker2, 0, Oben, Storage, mol);
    }

  Walker2 = *mol.start;

  while (Walker2.nr != int(Storage[0]))
    {
      Walker = *Walker.next;
    }
  
  helper.CopyVector(&Walker.x);
  helper.SubtractVector(&Walker2.x);
  Oben.ProjectOntoPlane(&helper);
  helper.VectorProduct(&Oben);

       Find_next_suitable_point(Walker, Walker2, *(mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom->nr == Walker.nr ? mol.ListOfBondsPerAtom[Walker.nr][i]->rightatom : mol.ListOfBondsPerAtom[Walker.nr][i]->leftatom), 0, &helper, &Oben, Storage, RADIUS, mol);
  Walker3 = *mol.start;
  while (Walker3.nr != int(Storage[0]))
    {
      Walker3 = *Walker3.next;
    }

  //Starting Triangle is Walker, Walker2, index Storage[0]

  AddPoint(&Walker);
  AddPoint(&Walker2);
  AddPoint(&Walker3);

  BPS[0] = new class BoundaryPointSet(&Walker);
  BPS[1] = new class BoundaryPointSet(&Walker2);
  BLS[0] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BPS[0] = new class BoundaryPointSet(&Walker);
  BPS[1] = new class BoundaryPointSet(&Walker3);
  BLS[1] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);
  BPS[0] = new class BoundaryPointSet(&Walker);
  BPS[1] = new class BoundaryPointSet(&Walker2);
  BLS[2] = new class BoundaryLineSet(BPS , LinesOnBoundaryCount);  

  BTS = new class BoundaryTriangleSet(BLS, TrianglesOnBoundaryCount);
  TrianglesOnBoundary.insert( TrianglePair(TrianglesOnBoundaryCount, BTS) );
  TrianglesOnBoundaryCount++;

  for(int i=0;i<NDIM;i++) 
    {
      LinesOnBoundary.insert( LinePair(LinesOnBoundaryCount, BTS->lines[i]) );
      LinesOnBoundaryCount++;
    };

       BTS->GetNormalVector(*BTS->NormalVector);

       if( BTS->NormalVector->ScalarProduct(&Oben)<0)
	 {
	   BTS->NormalVector->Scale(-1);
	 }
};


void Find_non_convex_border(Tesselation* Tess, molecule mol) 
{
  const double RADIUS =6;
  Tess->Find_starting_triangle(mol, RADIUS);

  for (LineMap::iterator baseline = Tess->LinesOnBoundary.begin(); baseline != Tess->LinesOnBoundary.end(); baseline++) 
    if (baseline->second->TrianglesCount == 1) 
      {
	Tess->Find_next_suitable_triangle(&mol, *(baseline->second), baseline->second->triangles.begin()->second, RADIUS); //the line is there, so there is a triangle, but only one.

      }
    else
      {
	printf("There is a line with %d triangles adjacent", baseline->second->TrianglesCount);
      }
};