#include "project.h"

/* fitness => forward = +1, collision = -1 */
/* action: 0 - forward, 1 - left, 2 - right, 3 - nop */
void decode( controller *ctrl, int *action, int *next_state, 
	     int input, int state ) {
   int i;
   int address = ((state<<1)+input)*(state_bits+action_bits);
   *action = (ctrl->fsa[address+state_bits]<<1) 
      + ctrl->fsa[address+state_bits+1];
   *next_state = 0;
   for ( i = 0; i < state_bits; i++ )
      *next_state += ctrl->fsa[address+i] << (state_bits-i-1);
}

void init_arrays() {
   int i, j;
   for ( i = 0; i < popsize; i++ ) {
      cur_pop[i] = (controller*)malloc(sizeof(controller));
      for ( j = 0; j < ctrl_bits; j++ ) {
	 cur_pop[i]->fsa[j] = random()%2;
      }
   }
   for ( i = 0; i < popsize + crossover_num + mutate_num; i++ ) {
      if ( i < popsize )
	 new_pop[i] = cur_pop[i];
      else
	 new_pop[i] = NULL;
   }
}

void free_arrays() {
   int i;
   for ( i = 0; i < popsize; i++ ) {
      if ( cur_pop[i] ) {
	 free( cur_pop[i] );
	 cur_pop[i] = NULL;
      }
   }
}

void rank_ctrl() {
   int i, j, index;
   int max_fit;
   int tmp_fit[popsize];
   
   for ( i = 0; i < popsize; i++ ) {
      tmp_fit[i] = cur_pop[i]->fitness;
   }
   
   for ( i = 0; i < popsize; i++ ) {
      max_fit = -1;
      index = -1;
      for ( j = 0; j < popsize; j++ ) {
	 if ( tmp_fit[j] > max_fit ) {
	    max_fit = tmp_fit[j];
	    index = j;
	 }
      }
      cur_rank[i] = index;
      tmp_fit[index] = -1;
   }
}

void rank_ctrl_new() {
   int i, j, index;
   int max_fit;
   int total = popsize + crossover_num + mutate_num;
   int tmp_fit[total];
   
   for ( i = 0; i < total; i++ ) {
      tmp_fit[i] = new_pop[i]->fitness;
   }
   
   for ( i = 0; i < total; i++ ) {
      max_fit = -1;
      index = -1;
      for ( j = 0; j < total; j++ ) {
	 if ( tmp_fit[j] > max_fit ) {
	    max_fit = tmp_fit[j];
	    index = j;
	 }
      }
      new_rank[i] = index;
      tmp_fit[index] = -1;
   }
}

int select_ctrl() {
   int i;
   while ( 1 ) {
      for ( i = 0; i < popsize; i++ ) {
	 if ( random()%100 <= choose )
	    return cur_rank[i];
      }
   }
}

void crossover_ctrl() {
   int cp = random()%ctrl_bits;
   int i, a, b;
   controller *ctrl_a, *ctrl_b, *new_ctrl;
   a = select_ctrl();
   b = select_ctrl();
   ctrl_a = cur_pop[cur_rank[a]];
   ctrl_b = cur_pop[cur_rank[b]];
   new_ctrl = (controller *)malloc(sizeof(controller));
   new_pop[new_popsize] = new_ctrl;
   for ( i = 0; i < cp; i++ ) {
      new_ctrl->fsa[i] = ctrl_a->fsa[i];
   }
   for ( ; i < ctrl_bits; i++ ) {
      new_ctrl->fsa[i] = ctrl_b->fsa[i];
   }
   cputs("ready");
   ds_passive(&SENSOR_2);
   wait_event(&dkey_released, KEY_PRGM); 
   msleep(250);  /* debounce the button */
   wait_event(&dkey_pressed, KEY_PRGM);  
   ds_active(&SENSOR_2);
   simulate(new_ctrl);   
   new_popsize++;
}

void mutate_ctrl() {
   int i, a;
   controller *ctrl_a, *new_ctrl;
   a = select_ctrl();
   ctrl_a = cur_pop[cur_rank[a]];
   new_ctrl = (controller *)malloc(sizeof(controller));
   new_pop[new_popsize] = new_ctrl;
   for ( i = 0; i < ctrl_bits; i++ ) {
      if ( random()%100 <= mutate )
	 new_ctrl->fsa[i] = 1- ctrl_a->fsa[i];
      else
	 new_ctrl->fsa[i] = ctrl_a->fsa[i];
   }
   cputs("ready");
   ds_passive(&SENSOR_2);
   wait_event(&dkey_released, KEY_PRGM); 
   msleep(250);  /* debounce the button */
   wait_event(&dkey_pressed, KEY_PRGM);  
   ds_active(&SENSOR_2);
   simulate(new_ctrl);
   new_popsize++;
}

void select_next_generation() {
   int i;
   cputs("select");
   sleep(2);
   for ( i = 0; i < popsize; i++ ) {
      cur_pop[i] = new_pop[new_rank[i]]; 
   }
   for ( ; i < new_popsize; i++ ) {
      free(new_pop[new_rank[i]]);
      new_pop[i] = NULL;
   }
}

void init_new_generation() {
   int i;
   int total =  popsize + crossover_num + mutate_num;
   for ( i = 0; i < total; i++ ) {
      if ( i < popsize )
	 new_pop[i] = cur_pop[i];
      else
	 new_pop[i] = NULL;
   }
   new_popsize = popsize;
}

void move_forward() {
   motor_control(fwd, fwd, FWD_SPEED, FWD_SPEED, 1000);
}

void turn_left() {
   motor_control(fwd, rev, FWD_SPEED, REV_SPEED, 1000);
}

void turn_right() {
   motor_control(rev, fwd, REV_SPEED, FWD_SPEED, 1000);
}

void reverse() {
   motor_control(rev, rev, FWD_SPEED, FWD_SPEED, 500);
   motor_control(rev, rev, FWD_SPEED/2, FWD_SPEED/2, 250);
   motor_control(rev, rev, MIN_SPEED, MIN_SPEED, 250);  
}

void stop(int sleep) {
   motor_control(fwd, fwd, 0, 0, sleep);
}

void motor_control( int dir_a, int dir_c, int speed_a, int speed_c,
		    int sleep ) {
   motor_a_dir(dir_a);
   motor_c_dir(dir_c);
   motor_a_speed(speed_a);
   motor_c_speed(speed_c);
   msleep(sleep);
}

wakeup_t turn(wakeup_t data) {
   int proc_pri = (int) data;
   if ( proc_pri >= cur_pri ) {
      cur_pri = proc_pri;
      return 1;
   }
   else {
      return 0;
   }
}

wakeup_t finish(wakeup_t data) {
   int proc_pri = (int) data;
   if ( proc_pri >= cur_pri ) {
      cur_pri = PRIO_LOWEST;
      return 1;
   }
   else {
      return 0;
   }
}

wakeup_t collision(wakeup_t data) {
   if ( TOUCH_1 )
      return 1;
   else if ( TOUCH_3 )
      return 3;
   else
      return 0;
}

wakeup_t obstacle(wakeup_t data) {
   return ( proximity > 0 );
}

int resolve() {
   int event;
   while ( 1 ) {
      event = wait_event(&collision, 0);
      wait_event(turn, PRIO_RESOLVE);
      get_control()->fitness--;
      reverse();
      if ( event == 1 ) {
	 turn_right();
      }
      else {
 	 turn_left();
      }
      stop(100);
      wait_event(&finish, PRIO_RESOLVE);
   }
   return 1;
}

void set_control(controller *ctrl) {
   cur_ctrl = ctrl;
}

controller* get_control() {
   return cur_ctrl;
}

int explore() {
   int action, next_state;
   int i;
   ready = 0;
   for ( i = 0; i < iterations; i++ ) {
      wait_event(&turn, PRIO_EXPLORE);
      lcd_int(proximity);
      decode( get_control(), &action, &next_state, 
	      (proximity>0), state);
      proximity = 0;
      state = next_state;
      if ( action == 0 ) {
	 get_control()->fitness++;
	 move_forward();
      }
      else if ( action == 1 ) {
	 turn_left();
      }
      else if ( action == 2 ) {
	 turn_right();
      }
      else if ( action == 3 ) {
	 stop(1000);
	 reverse();
      }
      stop(10);
      wait_event(&finish, PRIO_EXPLORE);
   }
   ready = 1;
   return 1;
}

int signal() {
   while(1){
      S_SR&=~(SSR_TRANS_EMPTY | SSR_TRANS_END);   // clean up Flags
      S_CR|=SCR_TRANSMIT;                         // enable Transmitter
      S_TDR = 0x00;          // transmit as many lightpulses as possible
      phase = SIGNAL;        // Indicate transmitting
      msleep(3);             // wait a little while
      phase = IDLE;          // Indicate Idle
      msleep(6);             // and wait again
   }
}

wakeup_t phase_signal(wakeup_t data) {
   return phase;
}

int ticker() {
   int cur=0, last=0, diff=0;  
   while(1){
      proximity = 0;
      while(phase == IDLE) {} // we have to wait until there is a  transmission
      while(phase == SIGNAL) {
	 detecting = 1;
	 last = cur;
	 cur = SENSOR_2>>7;
	 diff = last;
	 diff -= cur;
	 if(diff > PROX_THRESH) {
	    proximity++;
	 }
      }
      detecting=0;
   }
}

int avoid() {
   unsigned mode;
   while( 1 ) {
      wait_event(&obstacle, 0);
      wait_event(&turn, PRIO_AVOID);
      reverse();
      mode = random()%2;
      if ( mode == 0 ) {
	 turn_right();
      }
      else {
	 turn_left();
      }
      proximity=0;
      stop(100);
      wait_event(&finish, PRIO_AVOID);
   }
   return 1;
}

wakeup_t end_simulate(wakeup_t data) {
   return ready;
}

void simulate(controller *ctrl) {
   ctrl->fitness = 100;
   state = 0;
   cur_pri = PRIO_LOWEST;
   set_control(ctrl);
   pid0 = execi(&explore, 0, NULL, PRIO_EXPLORE, DEFAULT_STACK_SIZE);
   pid1 = execi(&resolve, 0, NULL, PRIO_RESOLVE, DEFAULT_STACK_SIZE);
   /* pid2 = execi(&avoid, 0, NULL, PRIO_AVOID, DEFAULT_STACK_SIZE); */
   pid3 = execi(&signal, 0, NULL, PRIO_AVOID, DEFAULT_STACK_SIZE);
   pid4 = execi(&ticker, 0, NULL, PRIO_AVOID, DEFAULT_STACK_SIZE);
   tm_start();
   sleep(1);
   wait_event(&end_simulate, 0);
   stop(1000);
   kill(pid0);
   kill(pid1);
   cur_pri = PRIO_LOWEST;
   lcd_int(ctrl->fitness);
   sleep(1); 
}

int main() {
   int gen, pop, i;
   srandom(sys_time);
   init_arrays();

   S_BRR = B2400;
   *((char*)&PORT4) &=~1;

   /* Prolog */
   for( pop = 0; pop < popsize; pop++ ) {
      cputs("ready");
      ds_passive(&SENSOR_2);
      wait_event(&dkey_released, KEY_PRGM); 
      msleep(250);  /* debounce the button */
      wait_event(&dkey_pressed, KEY_PRGM);  
      ds_active(&SENSOR_2);
      simulate(cur_pop[pop]);
   }
   rank_ctrl();
   new_popsize = popsize;

   for( gen = 0; gen < generations; gen++ ) {
      init_new_generation();
      for ( i = 0; i < crossover_num; i++ )
	 crossover_ctrl();
      for ( i = 0; i < mutate_num; i++ )
	 mutate_ctrl();
      rank_ctrl_new();
      select_next_generation();
   }

   free_arrays();
   return 1;
}