#include <stdio.h>

#define NUMSPEEDS 5 
#define NUMLANES 3

//distribute the initial density evenly
//(the alternative is to fill each lane 
//as full as possible)
//#define DISTRIBUTE

//is the desired distribution uniform
//or normal?
//#define UNIFRM_DIST

#define min(x,y) ((x<y)?x:y)
#define max(x,y) ((x<y)?y:x)
#define abs(x) ((x<0)?(-x):x)

int close(double x,double y) {
   return abs(x-y)<0.01;
}

#define bool int
#define false 0
#define true 1

double alpha = 2; //extra space each car takes up for lane changes.
double density[NUMLANES]; //the density of each lane
double rho[NUMLANES][NUMSPEEDS]; //the percentages of cars in
				//a particular lane going a 
				//particular speed.
double rho_base[NUMSPEEDS]; //the distribution of desired speeds 
                            //that is, this is how fast everyone
			    //would be going if there were no traffic.

//percentage chance that we move to the left or right if both
//are availible to us.
//frac_L+frac_R == 1
double frac_L = .5;
double frac_R = .5;
//chance we'll move right or left when we don't have to make a
//decision (frac_nd_L+frac_nd_R<=1)
double frac_nd_L = 0;
double frac_nd_R = 0;

double avg_speed() {
   double avg = 0;
   int i,j;
   for(i=0;i<NUMLANES;i++)
      for(j=0;j<NUMSPEEDS;j++)
	 avg += (double)(j) * rho[i][j];
   avg /= NUMLANES;
   return avg;
}
   
//returns an alpha dependent on the average speed.
//for the given lane
double getalpha(int l) {
   double al = 0;
   int i;
   for(i=0;i<NUMSPEEDS;i++)
      al+=rho[l][i];
   return alpha+al*alpha;
}

/**returns the percent chance that a car
 * going speed x will have to make a decision
 * (slow down or change lanes).
 * car is in lane l
 */
double D(int l, int x) {
   double slower = 0.;
   int i;
   for (i=0;i<x;i++)
      slower += rho[l][i];
   return max(0,min(getalpha(l)*slower*density[l],1));
}

/**returns the percent chance that a car 
 * going speed x will have to slow down.
 */
double S(int l, int x) {
   double slower = 0.;
   int i;
   slower = D(l,x);
   if (l > 0) slower *= max(0,min(getalpha(l-1)*density[l-1],1));
   if (l < NUMLANES-1) slower *= max(0,min(getalpha(l+1)*density[l+1],1));
   return slower;
}

/**returns the percent chance that a car
 * ging speed x will move into the right
 * lane.
 */
double R(int l, int x) {
   double decision = 0;
   double no_decision = 0;
   double d = D(l,x);
   //we'll never move right from the rightmost lane
   if (l == 0) return 0;
   //when we can move left too...
   if (l < NUMLANES-1) {
      //when both lanes are open and we have to make a decision,
      //move right frac_R the time.
      decision += (1-min(1,getalpha(l+1)*density[l+1]))*
	          (1-min(1,getalpha(l-1)*density[l-1]))*frac_R;
      //when the left lane isn't open, and the right lane is,
      //we always more right only when we have to make a decision
      decision += min(1,getalpha(l+1)*density[l+1])*
                  (1-min(1,getalpha(l-1)*density[l-1]));
   } else {
   	//when we can't move left, we move right if we have to 
   	//make a decision
   	decision += 1-min(1,getalpha(l-1)*density[l-1]);
   }
   //move right frac_nd_R percent of the time possible.
   no_decision += (1-min(1,getalpha(l-1)*density[l-1]))*frac_nd_R;
   return d*decision+(1-d)*no_decision;
}

//sum of R over all speeds
double R_h(int l) {
   int i;
   double ret = 0;
   for (i=0;i<NUMSPEEDS;i++)
      ret += R(l,i)*rho[l][i];
   return ret;
}

/**returns the percent chance that a car
 * going speed x will move left one lane
 */
double L(int l, int x) {
   double decision = 0;
   double no_decision = 0;
   double d = D(l,x);
   //we'll never move left from the leftmost lane
   if (l == NUMLANES-1) return 0;
   //when we can move right too...
   if (l > 0) {
      //when both lanes are open and we have to make a decision,
      //move left frac_L the time.
      decision += (1-min(1,getalpha(l+1)*density[l+1]))*
	          (1-min(1,getalpha(l-1)*density[l-1]))*frac_L;
      //when the right lane isn't open, and the left lane is,
      //we more left only when we have to make a decision
      decision += min(1,getalpha(l-1)*density[l-1])*
                  (1-min(1,getalpha(l+1)*density[l+1]));
   } else {
   	//when we can't move right, we move left if we have to 
   	//make a decision
   	decision += 1-min(1,getalpha(l+1)*density[l+1]);
   }
   //move left frac_nd_L percent of the time possible.
   no_decision += (1-min(1,getalpha(l+1)*density[l+1]))*frac_nd_L;
   return d*decision+(1-d)*no_decision;
}

//sum over all speeds
double L_h(int l) {
   int i;
   double ret = 0;
   for (i=0;i<NUMSPEEDS;i++)
      ret += L(l,i)*rho[l][i];
   return ret;
}

/** returns the % of cars in lane 
 * l going speed x which want to be going
 * faster.
 */
double W(int l,int x) {
   int i;
   double want = 0;
   for(i=x+1;i<NUMSPEEDS;i++)
      want += rho_base[i]-rho[l][i];
   want = min(want,rho[l][x]-rho_base[x]);
   want = max(want,0);
   return want;
}

//returns true if change occured...
bool calc_next_rho() {
   int i,j;
   double speed_up = 0;
   double slow_down = 0;
   double tmp[NUMLANES][NUMSPEEDS];
   bool ret = false;
   for(i=0;i<NUMLANES;i++) {
      //the percentage of cars that speed up
      speed_up = (1-D(i,0))*W(i,0);
      slow_down = rho[i][1]*S(i,1);

      //the new rho for the slowest
      tmp[i][0] = rho[i][0] + slow_down - speed_up;
      for(j=1;j<NUMSPEEDS-1;j++) {
	 tmp[i][j] = rho[i][j] - slow_down + speed_up;
	 speed_up = (1-D(i,j))*W(i,j);
	 slow_down = rho[i][j+1]*S(i,j+1);
	 tmp[i][j] -= speed_up;
	 tmp[i][j] += slow_down;
      }
      //the new rho for the fastest.
      tmp[i][NUMSPEEDS-1] = rho[i][NUMSPEEDS-1] - slow_down + speed_up;
   }
   for(i=0;i<NUMLANES;i++)
      for(j=0;j<NUMSPEEDS;j++) {
	 if (!close(tmp[i][j],rho[i][j])) 
	    ret = true;
	 rho[i][j] = tmp[i][j];
      }

   return ret;
}

//returns true if not at equalibrium
bool calc_next_density() {
   double tmp_den[NUMLANES];
   double chance_L, chance_R;
   bool ret = false;
   int i;
   for(i=0;i<NUMLANES;i++) {
      chance_L = density[i]*R_h(i);
      chance_R = density[i]*L_h(i);
      tmp_den[i] = density[i]-chance_L-chance_R;
      if (i>0) tmp_den[i] += density[i-1]*L_h(i-1);
      if (i<NUMLANES-1) tmp_den[i] += density[i+1]*R_h(i+1);
   } 
   for(i=0;i<NUMLANES;i++) {
      if (!close(density[i],tmp_den[i])) ret = true;
      density[i] = tmp_den[i];
   }
   return ret;

}

void printRHO() {
   int i,j;
   double sum;
   for(i=0;i<NUMLANES;i++) {
      printf("Lane %i, Density: %f\n",i,density[i]);
      for(j=0;j<NUMSPEEDS;j++) {
	 printf("%i: %f, ",j,rho[i][j]);
      }
      printf("\n");
   }
   printf("avg speed: %f\n",avg_speed());
   printf("\n");  
}

int main(int argn, char** argv) {
   int i,j;
   int t;
   int times = 10000;
   double tmp;

   if (argn != 6) {
      printf("arglist: traffic <density*numlanes> <getalpha()> <frac_R> <frac_nd_R> <frac_nd_L>.\nAll ints to be divided by 100\nexample: traffic 150 2 1 .75 .05");
      exit(1);
   }

   alpha = (double)(atoi(argv[2]))/100.0;

   frac_R = (double)(atoi(argv[3]))/100.0;
   frac_L = 1-frac_R;

   frac_nd_R = (double)(atoi(argv[4]))/100.0;
   frac_nd_L = (double)(atoi(argv[5]))/100.0;

#ifdef UNIFRM_DIST
   //initialize rho_base
   for(i=0;i<NUMSPEEDS;i++)
      rho_base[i] = 1.0/NUMSPEEDS;
#else
   rho_base[0] = .05;
   rho_base[1] = .2;
   rho_base[2] = .5;
   rho_base[3] = .2;
   rho_base[4] = .05;
#endif

   //initialize rho
   for(i=0;i<NUMLANES;i++)
      for(j=0;j<NUMSPEEDS;j++)
	 rho[i][j] = 1.0/NUMSPEEDS;

#ifdef DISTRIBUTE
   //initialize density
   tmp = (double)(atoi(argv[1]))/100.0;
   tmp = tmp/(double)(NUMLANES);
   for(i=0;i<NUMLANES;i++)
      density[i] = tmp;
#else
   for(i=0;i<NUMLANES;i++)
      density[i] = 0;
   density[0] = (double)(atoi(argv[1]))/100.0;
   i=0;
   while (density[i]>1) {
      if (i==NUMLANES-1) {
	 printf("Too much density.\n");
	 exit(1);
      }
      density[i+1] += density[i]-1;
      density[i] = 1;
      i++;
   }
#endif //DISTRIBUTE

   //run the sim
   for(t=0; t<times; t++) {
      //if (t%100 == 0) 
	 //printRHO();
      //calc next rho
      if (calc_next_rho() + calc_next_density() == 0) {
	 printf("Equlibrium achieved after %i runs!\n",t);
	 break;
      }
   }
   if (t==times) printf("Equlibrium not achieved in %i runs..\n",t);
   //print current rho
   printRHO();
   return 0;
}