#include <assert.h>

#include "IntracellularMolecule.h"

//#define DEBUG_INTERSECTIONS

//////////////////////////////////////////////////////////////////////
// Constructor for the IntracellularMolecule object
//

IntracellularMolecule::IntracellularMolecule() : Molecule(){}
IntracellularMolecule::IntracellularMolecule(Point & loc) : Molecule(loc){}


//////////////////////////////////////////////////////////////////////
// Destructor for the IntracellularMolecule object
//
IntracellularMolecule::~IntracellularMolecule(){

	// Destructing the IntracellularMolecule
	printf("Deleting IntracellularMolecule\n");
}


//////////////////////////////////////////////////////////////////////
// Get IntracellularMolecule type
//
moltype IntracellularMolecule::getType(){}



//////////////////////////////////////////////////////////////////////
// Get IntracellularMolecule type name
//
string IntracellularMolecule::getTypeName(){}

//////////////////////////////////////////////////////////////////////
// Get group
//
int IntracellularMolecule::getGroup(){
	return MOL_GROUP_INTRACELLULAR;
}



//////////////////////////////////////////////////////////////////////
// Get group name
//
string IntracellularMolecule::getGroupName(){
	return MOL_GROUP_NAME_INTRACELLULAR;
}

void IntracellularMolecule :: move(){
	float distance = RandomNG::randFloat(0.0,mobility);

	distance *= 1.0e2 * resolution * sqrt(TIMESCALE);

	move_recursive(distance);
}

void IntracellularMolecule :: move_recursive(float distance)
{
#ifdef DEBUG_INTERSECTIONS
  cout << "move_recursive(" << distance << ")" << endl;
#endif
	int x = RandomNG::randInt(-101,100);
	int y = RandomNG::randInt(-101,100);
	int z = RandomNG::randInt(-101,100);

	if (x == 0 && y == 0 && z == 0)
	{
	  return;
	}

	Vector3D unit_translation(x,y,z);
	unit_translation.normalize();
	
	Vector3D translation = unit_translation;
	translation.scalarize(distance);

	float intersections [((int) mobility + 2) * 3];

	int num_intersections;

	num_intersections = find_lattice_intersections(intersections, pos, translation);

	int cur_intersection_num;

	const static float INTERSECTION_EPSILON = 1.0e-3;

	for (cur_intersection_num = 0; cur_intersection_num < num_intersections; cur_intersection_num ++)
	{
	  Vector3D intersection_translation;

	  float intersection_distance;

	  intersection_distance = intersections[cur_intersection_num];

	  if (intersection_distance > distance)
	  {
	    // This shouldn't happen, but it does.
	    // An intersection should not have been detected if the particle isn't moving far enough to make the intersection.
	    // Fix this one day.

#ifdef DEBUG_INTERSECTIONS
	    cout << "intersection_distance (" << intersection_distance << ") > distance (" << distance << ")" << endl;
#endif	    
	    break;
	  }

	  intersection_distance += INTERSECTION_EPSILON;

	  intersection_translation = unit_translation;
	  intersection_translation.scalarize(intersection_distance);

	  Point intersection_location(roundToInt(pos.x + intersection_translation.x),
				      roundToInt(pos.y + intersection_translation.y),
				      roundToInt(pos.z + intersection_translation.z));

#ifdef DEBUG_INTERSECTIONS
	  cout << "(" << pos.x + intersection_translation.x << ", " << pos.y + intersection_translation.y << ", " << pos.z + intersection_translation.z << ") ==> (" << intersection_location.x << "," << intersection_location.y << "," << intersection_location.z << ") ";
#endif	  
	  int env = lEnv->getEnvironment(intersection_location);

#ifdef DEBUG_INTERSECTIONS
	  cout << "env at (" << intersection_location.x << "," << intersection_location.y << "," << intersection_location.z << ") = " << env << endl;
#endif	  
	  if((env!=-1) && lEnv->isCompatibleEnvironment(getType(),env)){
	    // Let molecule move within the lattice
	  }
	  else
	  {
#ifdef DEBUG_INTERSECTIONS
	    cout << "Collision detected with unfavourable environment at distance " << intersection_distance << endl;
#endif
	    intersection_distance = intersections[cur_intersection_num];
	    if (intersection_distance > INTERSECTION_EPSILON)
	    {
	      intersection_distance -= INTERSECTION_EPSILON;
	      intersection_translation = unit_translation;
	      intersection_translation.scalarize(intersection_distance);
	    
	      translate(intersection_translation);

#ifdef DEBUG_INTERSECTIONS
	      cout << "Collided, but moving to ("
		   << pos.x << ", "
		   << pos.y << ", "
		   << pos.z << ")" << endl;
#endif
	      assert (pos.x > -0.5);
	      assert (pos.y > -0.5);
	      assert (pos.z > -0.5);
	    }
	    else
	    {
#ifdef DEBUG_INTERSECTIONS
	      cout << "Collided, and was already at the collision site." << endl;
#endif
	      intersection_distance = 0.0;
	    }
	    move_recursive(distance - intersection_distance);
	    return;
	  }

	}

	Point new_location(roundToInt(pos.x + translation.x),
			   roundToInt(pos.y + translation.y),
			   roundToInt(pos.z + translation.z));

	int env = lEnv->getEnvironment(new_location);

	if((env!=-1) && lEnv->isCompatibleEnvironment(getType(),env)){
	  // Let molecule move within the lattice
	  translate(translation);
	}
	else
	{
	  // Otherwise, try again.
	  // This can happen even if there are no "intersections" with neighbouring lattice cells,
	  // if the move would cause the particle to move right to the border.
	  move_recursive(distance);
        }

#ifdef DEBUG_INTERSECTIONS
	cout << "Did not collide, but moving to ("
	     << pos.x << ", "
	     << pos.y << ", "
	     << pos.z << ")" << endl;
#endif
	assert (pos.x > -0.5);
	assert (pos.y > -0.5);
	assert (pos.z > -0.5);
}

int IntracellularMolecule :: find_lattice_intersections(float intersections [], Point pos, Vector3D translation)
{
  int num_intersections = 0;

  float translation_magnitude;
  int cur_intersection_num;

  translation_magnitude = sqrt (translation.x * translation.x + translation.y * translation.y + translation.z * translation.z);

#ifdef DEBUG_INTERSECTIONS
  cout << "(" << pos.x << "," << pos.y << "," << pos.z << ") + (" << translation.x << "," << translation.y << "," << translation.z << ")" << " = (" << pos.x + translation.x << "," << pos.y + translation.y << "," << pos.z + translation.z << ")" << endl;
#endif
  // The +0.5 is because rounding is done to identify the lattice cell, not truncation.

  num_intersections += find_lattice_intersections_one_axis(intersections, pos.x + 0.5, translation.x, translation_magnitude, num_intersections);
  num_intersections += find_lattice_intersections_one_axis(intersections, pos.y + 0.5, translation.y, translation_magnitude, num_intersections);
  num_intersections += find_lattice_intersections_one_axis(intersections, pos.z + 0.5, translation.z, translation_magnitude, num_intersections);

  qsort (intersections, num_intersections, sizeof (float), compare_float);

#ifdef DEBUG_INTERSECTIONS
  cout << "Intersections: ";
  for (cur_intersection_num = 0; cur_intersection_num < num_intersections; cur_intersection_num ++)
  {
    cout << intersections[cur_intersection_num] << " ";
  }
  cout << endl;
#endif

  return num_intersections;
}

int IntracellularMolecule :: find_lattice_intersections_one_axis(
  float intersections [],
  float pos_one_axis,
  float translation_one_axis,
  float translation_magnitude,
  int num_intersections)
{
  int num_one_axis_intersections;
  float one_axis_intersection_at;
  float axis_ratio;
  int cur_intersection_num;

  int num_new_intersections = 0;

  if (translation_one_axis == 0.0)
  {
    return 0;
  }
  
  axis_ratio = translation_magnitude / fabs (translation_one_axis);

  if (translation_one_axis > 0)
  {
    num_one_axis_intersections = (int) (translation_one_axis + (pos_one_axis - (int) pos_one_axis));
#ifdef DEBUG_INTERSECTIONS
    cout << num_one_axis_intersections << " = (int) (" << translation_one_axis << " + (" << pos_one_axis << " - (int) " << pos_one_axis << "))" << endl;
#endif
    one_axis_intersection_at = 1 - (pos_one_axis - (int) pos_one_axis);
  }
  else
  {
    num_one_axis_intersections = (int) (1 - (translation_one_axis + (pos_one_axis - (int) pos_one_axis)));
#ifdef DEBUG_INTERSECTIONS
    cout << num_one_axis_intersections << " = (int) (1 - (" << translation_one_axis << " + (" << pos_one_axis << " - (int) " << pos_one_axis << ")))" << endl;
#endif
    one_axis_intersection_at = (pos_one_axis - (int) pos_one_axis);
  }

  if (num_one_axis_intersections > 0)
  {
    intersections[num_intersections + num_new_intersections] = one_axis_intersection_at * axis_ratio;
    num_new_intersections ++;

    for (cur_intersection_num = 1; cur_intersection_num < num_one_axis_intersections; cur_intersection_num ++)
    {
      one_axis_intersection_at += 1.0;
      intersections[num_intersections + num_new_intersections] = one_axis_intersection_at * axis_ratio;
      num_new_intersections ++;
    }
  }  

  return num_new_intersections;
}

int IntracellularMolecule :: compare_float (const void * va, const void * vb)
{
  float a = *((float *) va);
  float b = *((float *) vb);

  if (a > b)
  {
    return 1;
  }
  else if (a < b)
  {
    return -1;
  }
  else
  {
    return 0;
  }
}
