#include <stdio.h>
#include <stdlib.h>
#include <assert.h>
#include <math.h>
#include <float.h>
#include <sys/types.h>
#include <unistd.h>

#define VMIN 3.0
#define VMAX 10.0

#define RADIUS 250

#define VRATIO 200   // speed report frequency
#define PRATIO 2000  // progress report frequency


#define PI 3.141592653

#define SIMTIME 2147483647 // 2^31 - 1 (largest 32-bit signed integer)

#define NUM_ITR 500
#define TNUM_ITR 360

#define TINC 0.05 // time increment

#define DEBUG 0 

typedef enum {CIRCLE, SQUARE} areatype;
typedef enum {false, true} bool;

double WIDTH, RWP_FACTOR, AREA;

areatype atype = SQUARE;

typedef struct {
   double x;
   double y;
} location;

typedef struct {
   unsigned int id;
   double v;
   location *d;  // destination point
   location *c;  // current position 
} node;

/*
 * Forward declarations
 */
void randloc(location *p);
double drange(double min, double max);
node *new_node(unsigned int id);
void assign_loc(location *d, location *s);
double dist(location *p1, location *p2);

double drange(double min, double max)
{
   return min + (max-min) * drand48();
}

node *new_node(unsigned int id)
{
   node *np = (node *) malloc(sizeof(node));
   assert(np != NULL);

   np->id = id;

   np->d = (location *) malloc(sizeof(location));
   np->c = (location *) malloc(sizeof(location));

   assert(np->d != NULL);
   assert(np->c != NULL);

   randloc(np->c);

   randloc(np->d);

   return np;
}

/*
 * randloc(location *p) -- Assign random coordinates to p from a square region
 */
void randloc(location *p)
{
   if ( atype == SQUARE ) {
      p->x = drange(0, WIDTH);
      p->y = drange(0, WIDTH);
   } else if ( atype == CIRCLE ) {
      do {
         p->x = drange(-WIDTH, WIDTH);
         p->y = drange(-WIDTH, WIDTH);
      } while ( p->x * p->x + p->y * p->y > WIDTH * WIDTH );
   } 
}

void assign_loc(location *d, location *s)
{
   d->x = s->x;
   d->y = s->y;
}

// dist(location *p1, location *p2) -- calculate distance between two locations
double dist(location *p1, location *p2)
{
   double x1, y1, x2, y2;

   assert(p1 != NULL);
   assert(p2 != NULL);

   x1 = p1->x;
   y1 = p1->y;

   x2 = p2->x;
   y2 = p2->y;

   return sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2) );
}

/*
 * relspeed(node *np1, node *np2) -- calculate the relative speed of two nodes
 */
double relspeed(node *np1, node *np2)
{
   location *p1;
   location *d1;
   location *p2;
   location *d2;
   double v1, v2;

   double r1, r2, sin1, sin2, cos1, cos2, vbar;

   p1 = np1->c;
   d1 = np1->d;
   v1 = np1->v;

   p2 = np2->c;
   d2 = np2->d;
   v2 = np2->v;

   r1 = dist(p1, d1);
   r2 = dist(p2, d2); 

   if ( r1 == 0 || r2 == 0 ) {
      return 0.0; 
   }

   cos1 = (d1->x - p1->x) / r1;
   sin1 = (d1->y - p1->y) / r1;

   cos2 = (d2->x - p2->x) / r2;
   sin2 = (d2->y - p2->y) / r2;

   //vbar = sqrt( pow(v2*sin2 - v1*sin1, 2) + pow(v2*cos2 - v1*cos1, 2) );
   vbar = sqrt( (v2*sin2 - v1*sin1)*(v2*sin2 - v1*sin1) + (v2*cos2 - v1*cos1)*(v2*cos2 - v1*cos1) );

   return vbar;
}

/* 
 * void advance(node *np) -- advance node according to its destination and current speed. 
 * If close enough, just jump to destination.
 */
void advance (node *np)
{
   location *p;  // current position
   location *d;  // destination
   double v;     // current speed
   double r, l, sin, cos;

   p = np->c;
   d = np->d;
   v = np->v;

   if ( fabs(p->x - d->x) <= DBL_EPSILON && fabs(p->y - d->y) <= DBL_EPSILON ) {
      return;
   }

   r = dist(p, d); 

   cos = (d->x - p->x) / r;
   sin = (d->y - p->y) / r;
   l = v * TINC;

   p->x = p->x + l * cos;
   p->y = p->y + l * sin;

   return;
}

/*
 * angle () -- calculate the relative angle between the velocities of two nodes.
 * We use atan2 to return signed angle from [-pi, pi].
 */
double angle(node *np1, node *np2)
{
   location *p1;
   location *d1;
   location *p2;
   location *d2;

   location vec1;
   location vec2;
   double dot, det;
   double a;

   p1 = np1->c;
   d1 = np1->d;

   p2 = np2->c;
   d2 = np2->d;

   vec1.x = d1->x - p1->x;
   vec1.y = d1->y - p1->y;

   vec2.x = d2->x - p2->x;
   vec2.y = d2->y - p2->y;

   dot = vec1.x*vec2.x + vec1.y*vec2.y;  // dot product 
   det = vec1.x*vec2.y - vec1.y*vec2.x;  // determinant

   a = atan2(det, dot); 

   return a;
}

int main ( int argc, char *argv[] )
{
   unsigned long int i, j, k, l; 
   double t1, t2, ct, nt, rt;
   double count;
   double vbar;
   double rv;
   double vsum = 0.0;
   double vmin, vmax;
   double tolerance;
   float progress;

   double last_meet = 0.0;
   double first_meet = 0.0;
   double rate, rsum = 0.0;
   double nbr,  nsum = 0.0;
   double theta, tsum = 0.0;
   int rcount = 0;
   int ncount = 0;
   int tcount = 0;

   FILE *rfp;   // log file for rate
   FILE *nfp;   // log file for contact time
   FILE *lfp;   // log file for epoch length
   FILE *vfp;   // log file for velocity
   FILE *tfp;   // log file for relative angle theta
   FILE *afp;   // log file for analysis

   node *np1 = new_node(1);
   node *np2 = new_node(2);

   bool in_range;

   unsigned long seed = (unsigned long) time(NULL);
   seed += 7 * (unsigned long) getpid();
   srand48(seed);

   if ( argc >= 3 ) {
      vmin = atof(argv[1]);
      vmax = atof(argv[2]);
   } else {
      vmin = VMIN;
      vmax = VMAX;
   }

   if ( atype == SQUARE ) {
      WIDTH = 6000.0;
      AREA  = WIDTH*WIDTH;
      RWP_FACTOR = 1.3683; 
   } else if ( atype == CIRCLE ) {
      WIDTH = 3385.1375 ;
      AREA  = PI * WIDTH * WIDTH ;
      RWP_FACTOR = 1.3333;
   } else {
      fprintf(stderr, "Un-supported area type: %d\n", atype);
      exit(1);
   }

   tolerance = 2 * VMAX * TINC;

   rfp = fopen("logs/mobrate.log", "w");
   nfp = fopen("logs/mobnbr.log", "w");
   lfp = fopen("logs/moblen.log", "w");
   vfp = fopen("logs/vbar.log", "w");
   tfp = fopen("logs/theta.log", "w");
   afp = fopen("logs/mob-analysis.log", "w");

   assert(rfp != NULL);
   assert(nfp != NULL);
   assert(lfp != NULL);
   assert(vfp != NULL);
   assert(tfp != NULL);
   assert(afp != NULL);

   fprintf(lfp, "%g\n", dist(np1->c, np1->d));
   fprintf(lfp, "%g\n", dist(np2->c, np2->d));

   np1->v = drange(vmin, vmax);
   np2->v = drange(vmin, vmax);

   if ( dist(np1->c, np2->c) <= RADIUS ) {
      in_range = true;
   } else {
      in_range = false;
   }

   double sin1, sin2;

   for ( i = 0; i < SIMTIME; i++ ) {
      vbar = relspeed(np1, np2); 
      theta = angle(np1, np2); 

      tsum += theta;
      tcount++;

      if ( i % VRATIO == 0 ) {
         fprintf(vfp, "%g\n", vbar);
         fprintf(tfp, "%g\n", theta);
      }

      if ( i % PRATIO == 0 ) {
         progress = (float) 100 * ( (double) i) / SIMTIME;
         printf("\b\b\b\b\b\b");
         printf("%.2f%%", progress);
         fflush(stdout);
      }

      vsum += vbar; 

      advance(np1);
      advance(np2);
      ct = i * TINC; // current time

      if ( in_range == true ) {
         if ( dist(np1->c, np2->c) > RADIUS ) { // going out of radio range
            nt = ct - first_meet ;
            last_meet = ct - TINC;
            in_range = false;
            fprintf(nfp, "%g\n", nt);
            nsum += nt;
            ncount++;
         }
      } else {
         if ( dist(np1->c, np2->c) <= RADIUS ) { // going into radio range
            rt = ct - last_meet ; // calculate and record rate (inter-contact time)
            first_meet = ct;
            in_range = true;

            fprintf(rfp, "%g\n", rt);
            rsum += rt;
            rcount++;
         }
      }

      if ( dist(np1->c, np1->d) < tolerance ) { // have we reached destination?
         assign_loc(np1->c, np1->d);

         randloc(np1->d);             // assign a new destination
         np1->v = drange(vmin, vmax); // assign new speed
         fprintf(lfp, "%g\n", dist(np1->c, np1->d));
      }

      if (  dist(np2->c, np2->d) < tolerance ) { // have we reached destination?
         assign_loc(np2->c, np2->d);

         randloc(np2->d);          // assign a new destination
         np2->v = drange(vmin, vmax); // assign new speed
         fprintf(lfp, "%g\n", dist(np2->c, np2->d));
      }
   }

   rv = vsum / i;
   vbar = (vmax-vmin)/log(vmax/vmin);
   rate = rsum / rcount; 
   nbr  = nsum / ncount; 
   theta = tsum / tcount;
   fprintf (afp, "exp rel-speed: %g speed: %g norm: %g gain: %g\n", rv, vbar, rv/vbar, RWP_FACTOR * rv / vbar);
   fprintf (afp, "exp rate: %g nbr: %g\n", rate, nbr );
   fprintf (afp, "exp angle: %g\n", theta);
   rate = 2 * RWP_FACTOR * rv * RADIUS / AREA;
   nbr  = PI * RADIUS / (2 * rv);
   fprintf ( afp, "sem rate: %g, nbr: %g (theoretical from experiment)\n", 1/rate, nbr );

   return 0;
}