#include <math.h>
#include <stdlib.h>
#include <time.h>
#include <stdio.h>
#include <windows.h>
#include "graph.h"


// class VECTOR
//

double VECTOR::Distance(const VECTOR &v) const
{
  return sqrt((x - v.x) * (x - v.x)  +  (y - v.y) * (y - v.y));
}
double VECTOR::Abs(void) const
{
  return sqrt(x * x  +  y * y);
}

VECTOR &VECTOR::operator +(const VECTOR &v) const
{
  return *new VECTOR(x + v.x, y + v.y);
}
VECTOR &VECTOR::operator -(const VECTOR &v) const
{
  return *new VECTOR(x - v.x, y - v.y);
}
VECTOR &VECTOR::operator *(const VECTOR &v) const
{
  return *new VECTOR(x * v.x, y * v.y);
}
VECTOR &VECTOR::operator /(const VECTOR &v) const
{
  return *new VECTOR(x / v.x, y / v.y);
}
VECTOR &VECTOR::operator *(double s) const
{
  return *new VECTOR(s * x, s * y);
}
VECTOR &VECTOR::operator /(double s) const
{
  return *new VECTOR(x / s, y / s);
}
VECTOR &VECTOR::operator +=(const VECTOR &v)
{
  x += v.x, 
  y += v.y;
  return *this;
}
VECTOR &VECTOR::operator -=(const VECTOR &v)
{
  x -= v.x, 
  y -= v.y;
  return *this;
}
VECTOR &VECTOR::operator /=(double s)
{
  x /= s;   y /= s;
  return *this;
}

VECTOR &VECTOR::Normalize(void) const
{
  double distance = sqrt(x * x  + y * y);
  return *new VECTOR(x / distance, y / distance);
}

VECTOR &operator *(double s, const VECTOR &vector)
{
  return *new VECTOR(s * vector.x, s * vector.y);
}

double VECTOR::Angle(const VECTOR &vector)
{
  double phi1 = atan2(y, x);
  double phi2 = atan2(vector.y, vector.x) - phi1;
  if (phi2 < 0.0) 
    phi2 += 6.2832;
  return phi2;   
}

#define ARENA_SIZE 32000

void *VECTOR::operator new(size_t s)
{
   static char *arena = NULL;
   static char *p;

   if (! arena)
     arena = p = new char[ARENA_SIZE];
   
   if (p - arena >= ARENA_SIZE - s)
     p = arena;
   
   void *tmp = p;
   p += s;
   return tmp;
}



// class GRAPH
//

GRAPH::GRAPH(void)
{
   first = NULL;  n = 0;

   maxRandom = 0.9;
   c1 = 0.8;
   c2 = 6.0;
   c3 = 35.0;
   stepConstantC4 = 0.1;
   osciConstantC5 = 0.5;
   skewConstantC6 = 0.5;
   rotaConstantC7 = 0.5;

   c8 = 0.3;

   iteration = 0;
}

void GRAPH::SetBaryCenter(void)
{
  int n;  NODE *node;
  
  baryCenter = VECTOR(0.0, 0.0);

  for (node = First(), n = 0;  node;  node = node->Next(), n++)
    baryCenter += node->Center();

  baryCenter /= (double) n;
}

  
int *GRAPH::CreateRandomIndices(int n)
{
  static int *indices = NULL;   int i, j, k;

  if (! indices)
    indices = new int[n];

  srand((unsigned) time( NULL ));
    
  for (i = 0;  i < n;   )
  {
    k = MulDiv(rand(), n - 1, RAND_MAX);

    for (j = 0;  j < i;  j++)
       if (indices[j] == k)
         break;
      
    if (j == i)
      indices[i++] = k;
  }      

  return indices;
}



//
//  F = c1 * (u - v) / dist(u, v)  * log(dist(u, v) / c2)
//
VECTOR &GRAPH::AttractiveForce(NODE &node1, NODE &node2)
{
  const VECTOR &u = node2.Center();
  const VECTOR &v = node1.Center();

  if (u == v)
    return *new VECTOR(0.0, 0.0);

  double distance = u.Distance(v);

  return c1 * (v - u) / (distance * log(distance / c2));
} 

//
//  F = (u - v) / dist(u, v) * c3 * dist(u, v)
//
VECTOR &GRAPH::RepulsiveForce(NODE &node1, NODE &node2)
{
  const VECTOR &u = node2.Center();
  const VECTOR &v = node1.Center();

  if (u == v)
    return *new VECTOR(0.0, 0.0);

  double distance = u.Distance(v);
   
 return c3 * (u - v) / (distance * distance * distance);
}


VECTOR &GRAPH::GravitationalForce(NODE &node)
{
  return c8 * (baryCenter - node.Center()) * node.Mass();
}

VECTOR &GRAPH::ComputeNodeImpulse(NODE &node)
{
  NODE *otherNode;    int k;

  VECTOR resultForce(0.0, 0.0);

  // Iterate through all adjacent edges to calculate attractive forces
  for (k = 0;  k < node.n;  k++) 
  {
    EDGE *edge = node.edges[k];
    otherNode = edge->TargetNode();

    if (otherNode == & node)
      otherNode = edge->SourceNode();

    resultForce += AttractiveForce(node, *otherNode);
  } 

  //Iterate through all other nodes to calculate repulsive forces 
  for (otherNode = First();  otherNode;  otherNode = otherNode->Next())
     if (otherNode != & node)
       resultForce -= RepulsiveForce(node, *otherNode);

  // Finally add the random force and the gravitational force
  resultForce += GravitationalForce(node);

  node.force = resultForce.Abs();  // FIXME

  resultForce += RandomForce(node);

  // we only need the impulse
  return resultForce.Normalize();
}

void GRAPH::AdjustTemperatureAndSkew(NODE &node, double angle)
{
   // Note that angle is a positive value between 0 and 2 pi (6.283...).
   // Hence 45 degree is 0.707 and -45 degree is 6.238 - 0.707 = 5.576
   
   if (angle < 0.7854 || angle > 5.4978 || (angle > 2.3562  &&  angle < 3.927))
   {
     // between -0.707 amd 0.707 (-45 mad 45 degree), there is acceleration,
     // between 2.434 and 3.848 (145 and 225 degree) there is oscillation
     
     node.temperature += osciConstantC5 * cos(angle);
   }         
   else 
   {
     // in the other ranges of the angle, there is rotation
     node.skew += skewConstantC6 * sin(angle);
     node.temperature -= rotaConstantC7 * fabs(node.skew);
   }    
   if (node.temperature < 0.0)
     node.temperature = 0.0;
   if (node.temperature > 1.0)
     node.temperature = 1.0;

}
 
double GRAPH::TemperatureSum(void)
{
  double temperatureSum = 0.0;

  for (NODE *node = First();  node;  node = node->Next())
     temperatureSum += node->temperature;
 
  return temperatureSum;
}

double GRAPH::ForceSum(void)
{
  NODE *node;   double fs = 0.0;   int n;
  for (node = First(), n = 0;  node;  node = node->Next(), n++)
     fs += node->force;
   return fs / (double) n;
}

NODE *GRAPH::Get(int k)
{
  NODE *node = NULL;

  for (node = First();  node;  node = node->Next())
     if (--k < 0) break;

  return node;
}     


void GRAPH::MainIteration(void)
{
     int *randIndex = CreateRandomIndices(n);

     for (int k = 0;  k < n;  k++)
     {
       NODE *node = Get(randIndex[k]);

       VECTOR &impulse = ComputeNodeImpulse(*node);

    // printf("\n     impulse = (%f, %f)", impulse.x, impulse.y);


       if (node->HasMoved())
         AdjustTemperatureAndSkew(*node, node->lastImpulse.Angle(impulse)); 
     
       node->MoveBy(stepConstantC4 * impulse.x * node->temperature,
                    stepConstantC4 * impulse.y * node->temperature);
       
       node->lastImpulse = impulse;
     }
     if (++iteration < 100)
       c3 += 0.1;
}

GRAPH &GRAPH::operator +=(NODE &node)
{
  for (NODE *e = First();  e && e->Next();  e = e->Next())
     ;
     
  if (e)
    e->next = & node;
    else first = & node;
  n++;
  return *this;
}   


void GRAPH::Print(void)
{
  int k = 0;
  
  for (NODE *node = First();  node;  node = node->Next())
     printf("\nnode %d, x = %f, y = %f, t = %f", 
             ++k, node->Center().x, node->Center().y, node->temperature);         
}



// class NODE
//

NODE::NODE(void) : center(0.0, 0.0), temperature(0.9), next(NULL), lastImpulse(0.0, 0.0)
{
    name = "?";     skew = 0.0;   n = 0;   edges = new EDGE *[10];
    flags = 0;
}
NODE::NODE(char *_name, double x, double y)
               : name(_name), center(x, y), temperature(0.9), next(NULL), lastImpulse(0.0, 0.0)
{
    skew = 0.0;   n = 0;   edges = new EDGE *[10];   flags = 0;
}

void NODE::Connect(NODE &node)
{
  EDGE *tmp = new EDGE(this, & node);
  edges[n++] = node.edges[node.n++] = tmp;
}


void NODE::MoveBy(double dx, double dy)
{
  center.x += dx;
  center.y += dy;
  MarkAsMoved();
}

double NODE::Mass(void)
{
  return n / 3.0;
}