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

inline int round(double x)
{
  //brauche ich das hier? Kann ich mit int operieren?
  return int(x > 0.0 ? x + 0.5 : x - 0.5);
}




void Find_next_suitable_point(atom a, atom b, atom Candidate, int n, Vector *d1, Vector *d2, double *Storage, const double RADIUS)
{
  /* 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.ScalarProduct(d1));
	    Storage[2]=AngleCheck.Angle(d2);
	  }
      }
  }

  if (n<5)
    {
      for(i=0; i<molecule.NumberOfBondsPerAtom[Candidate.nr];i++)
	{
	  while (Candidate.nr != molecule->ListOfBonds[Candidate.nr][i])
	    {
	      Walker = Walker.next;
	    }

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


void Find_next_suitable_triangle(Triangle T, BoundaryLine Line)
{
  Vector CenterOfLine = Line->endpoints.node[0].x;
  Vector direction1;
  Vector direction2;
  Vector helper;

  double *Storage[3];
  Storage[0]=-1;   // Id must be positive, we see should nothing be done
  Storage[1]=-2;   // 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, though due to Storage[1] this is of no concern

  
  helper.CopyVector(Line->endpoints[0].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].x);
	  break;
	}
    }


  direction1.CopyVector(Line->endpoints.node[0].x);
  direction1.Subtract(Line->endpoints.node[1].x);
  direction1.Crossproduct(T.NormalVector);

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

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


void Find_starting_triangle()
{
  atom Walker;
  atom Walker2;
  int max_index[3];
  double max_coordinate[3];
  Vector Oben;
  Vector helper;

  Oben.Zero;


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

  Walker = molecule->start;
  while (Walker->next != NULL)
    {
      for (i=0; i<3; i++)
	{
	  if (Walker.x[i]>max_coordinate[i])
	    {
	      max_coordinate[i]=Walker.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 = molecule->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< molecule->NumberOfBondsPerAtoms[Walker.nr]; i++)
    {
      Walker2 = molecule->start;
      while (Walker2.nr != molecule->ListOfBondsPerAtoms[Walker.nr][i])  // Stimmt die Ueberpruefung $$$
	{
	  Walker2 =Walker2->next;
	}

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

  Walker2=molecule->start;

  while (Walker2.nr != int(Storage[0]))
    {
      Walker = Walker.next;
    }
  
  helper.copyVector(Walker.x);
  helper.Subtract(Walker2.x);
  Oben.ProjectOntoPlane(helper.x);
  helper.VectorProduct(Oben);

  Find_next_suitable_point(Walker, Walker2, Candidate, 0, helper, Oben, Storage, Radius);

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

}


void Find_non_convex_border()
{

}