/*************************************/
/*   Albert Lin, Paul Wanda, Ran Tao */
/*   	(albybaby, pwanda, ran)		 */
/*         6.270 ROBOT CODE		 */
/*						 */
/*************************************/




/*********************************/
/*         MAIN FUNCTION         */
/*********************************/
void main()
{
  int start_port=6;
  int orientation=0;
  int counter=0;
  int pid_1, pid_2;

  /*insert calibration code here*/   
  start_machine2(start_port,10);
  orientation = detect_orientation();
 
  while (orientation==0&&counter<5)
    {
      orientation=detect_orientation();
      counter++;
    }
  turn_robot(orientation);
  if (orientation!=0)
    {
      pid_1 = start_process(second_do_strategy());
      msleep(1000L);
      pid_2 = start_process(main_do_strategy());
    }

  ao();
}


void main_test()
{
  int powerl=100;
  int powerr=0;
  
  for(powerr=100; powerr>=50; powerr=powerr-10)
	{	
		while(start_button()!=1)
		{}
		printf("Fwd 36 w/ arm. Lt=%d Rt=%d.\n", powerl, powerr);
  		main_move_forward_ARM2(36.0, 30.0, powerl, powerr);		
		/*main_move_back_ARM2(36.0, 30.0, powerl, powerr);*/
	}
	powerr=100;
  for(powerl=100; powerl>=50; powerl=powerl-10)
	{	
		while(start_button()!=1)
		{}
		printf("Fwd 36 w/ arm. Lt=%d Rt=%d.\n", powerl, powerr);
  		main_move_forward_ARM2(36.0, 30.0, powerl, powerr);	
		/*main_move_back_ARM2(36.0, 30.0, powerl, powerr);*/
	}

}


void turn_robot(int orientation)
{
  if (orientation!=0)
    {
      if (orientation==1)
	{
	  /*do nothing*/
	  return;
	}
      else if (orientation==2)
	{
	  /*120 has the effect of 90*/
	  main_orient_turn_counterclockwise(90.0);
	}
      else if (orientation==3)
	{
	  main_orient_turn_counterclockwise_180();	  
	}
      else if (orientation==4)
	{
	  /*120 has the effect of 90*/
	  main_orient_turn_clockwise(90.0);
	}
      else
	{
	  /*this is an error*/
	  return;
	}
    }
  else 
    return;
}

void main_do_strategy()
{
  /*presumably robot is already turned to correct orientation*/
  /*presumably, secondary robot has been deployed, and main robot is left alone facing East*/  
  main_motor_arm(-100);
  main_move_off_ramp(24.0);
  main_motor_arm(0);
  main_move_forward(12.0);
  main_deploy_arm();
  main_move_forward_ARM(36.0);
  main_turn_back_to_wall();
  main_move_forward_ARM(30.0);
  main_final_turn();
}

void main_move_forward(float dist)
{
  float speed=30.0;
  long timeon=(0L);

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*(1000.0));
  else
    timeon=(0L);

  main_motor_left(100);
  main_motor_right(90);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}

void main_move_off_ramp(float dist)
{
  float speed=30.0;
  long timeon=(0L);

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*(1000.0));
  else
    timeon=(0L);

  main_motor_left(80);
  main_motor_right(71);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}


void main_move_forward_ARM(float dist)
{
  float speed=30.0;
  long timeon=(0L);

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*(1000.0));
  else
    timeon=(0L);

  main_motor_left(100);
  main_motor_right(65);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}

 void main_move_forward_ARM2(float dist,float speed, int powerl, int powerr)
{
  /*float speed=30.0;*/
  long timeon=(0L);

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*(1000.0));
  else
    timeon=(0L);

  main_motor_left(powerl);
  main_motor_right(powerr);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}


void main_move_back(float dist)
{
  float speed=30.0;
  long timeon=0L;

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*1000.0);
  else
    timeon=0L;

  main_motor_left(-100);
  main_motor_right(-90);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}

void main_move_back_ARM(float dist)
{
  float speed=30.0;
  long timeon=0L;

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*1000.0);
  else
    timeon=0L;

  main_motor_left(-100);
  main_motor_right(-65);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}

void main_move_back_ARM2(float dist, float speed, int powerl, int powerr)
{
  /*float speed=30.0;*/
  long timeon=0L;

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*1000.0);
  else
    timeon=0L;

  main_motor_left(powerl);
  main_motor_right(powerr);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}


void main_orient_turn_clockwise(float degree)
{

  float speed=225.0; /*  225.0 is good */
  long timeon=0L;

  if (speed!=0.0)
    timeon=(long)((degree/speed)*1000.0);
  else
    timeon=0L;

  main_motor_left(75);
  main_motor_right(-75);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}

void main_orient_turn_counterclockwise(float degree)
{
  float speed=215.0; /*   215.0 is good???   */
  long timeon=0L;

  if (speed!=0.0)
    timeon=(long)((degree/speed)*1000.0);
  else
    timeon=0L;

  main_motor_left(-75);
  main_motor_right(75);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}


void main_orient_turn_counterclockwise_180()
{
  float speed=200.0;
  long timeon=0L;

  if (speed!=0.0)
    timeon=(long)((180.0/speed)*1000.0);
  else
    timeon=0L;

  main_motor_left(-65);
  main_motor_right(65);

  msleep(timeon);

  main_motor_left(0);
  main_motor_right(0);
}

void main_turn_back_to_wall()
{
  long timeon=1000L;
  main_motor_left(-100);
  main_motor_right(-25);
  msleep(timeon);
  main_motor_left(0);
  main_motor_right(0);
}

void main_final_turn()
{
  long timeon=1000L;

  main_motor_left(75);
  main_motor_right(22);
  msleep(timeon);
  main_motor_left(100);
  main_motor_right(65);
  msleep(1000L);
  main_motor_left(0);
  main_motor_right(0);
}

void main_motor_left(int power)
{  
  int motor_port=1;
  if (power==0)
    off(motor_port); 
  else if (power<110&&power>-110)
    motor(motor_port,power);
  else
    off(motor_port);
}

void main_motor_right(int power)
{  
  int motor_port=2;
  if (power==0)
    off(motor_port);
  else if (power<110&&power>-110)
    motor(motor_port,power);
  else
    off(motor_port);
}

void main_deploy_arm()
{
  main_motor_arm(100);
  msleep(500L);
  main_motor_arm(0);
}

void main_lock_arm()
{
  main_motor_arm(-100);
  msleep(500L);
  main_motor_arm(0);
}

void main_motor_arm(int power)
{
  int arm_port=5;
  if (power==0)
    off(arm_port);
  else if (power<110&&power>-110)
    motor(arm_port,power);
  else
    off(arm_port);
}


void second_do_strategy()
{
  second_motor_arm(-100);
  second_move_from_ramp(12.0);
  second_move_forward(8.0);
  second_motor_arm(0);
  second_deploy_arm();
  second_move_forward(24.0);
}

void second_move_forward(float dist)
{
  float speed=24.0;
  long timeon=0L;

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*1000.0);
  else
    timeon=0L;

  second_motor(100);

  msleep(timeon);

  second_motor(0);
}

void second_move_from_ramp(float dist)
{
  float speed=24.0;
  long timeon=0L;

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*1000.0);
  else
    timeon=0L;

  second_motor(50);

  msleep(timeon);

  second_motor(0);
}

void second_move_back(float dist)
{
  float speed=24.0;
  long timeon=0L;

  if (speed!=0.0) 
    timeon=(long)((dist/speed)*1000.0);
  else
    timeon=0L;

  second_motor(-100);

  msleep(timeon);

  second_motor(0);
}

void second_deploy_arm()
{
  second_motor_arm(100);
  msleep(1000L);
  second_motor_arm(0);
}

void second_lock_arm()
{
  second_motor_arm(-100);
  msleep(1000L);
  second_motor_arm(0);
}

void second_motor(int power)
{
  int motor_port=3;
  if (power==0)
    off(motor_port);
  else if (power<110&&power>-110)
    motor(motor_port,power);
  else
    off(motor_port);
}

void second_motor_arm(int power)
{
  int arm_port=4;
  if (power==0)
    off(arm_port);
  else if (power<110&&power>-110)
    motor(arm_port,power);
  else
    off(arm_port);
}



/**********************/
/****Color Sensors ****/
/**********************/

int sensor_color_NW()
{
  int analog_port=22; /*update!!*/
  defer();
  analog(analog_port);
  return analog(analog_port);
}

int sensor_color_NE()
{
  int analog_port=20; /*update!!*/
  defer();
  analog(analog_port);
  return analog(analog_port);
}

int sensor_color_SW()
{
  int analog_port=16; /*update!!*/
  defer();
  analog(analog_port);
  return analog(analog_port);
}

int sensor_color_SE()
{
  int analog_port=18; /*update!!*/
  defer();
  analog(analog_port);
  return analog(analog_port);
}

int detect_orientation()
{  
  int orientation=0;

  /*it's only important to detect the "odd one out" and not neccessary to determine exactly white or black for orientation*/

  /*return code: 
    1 our N is facing board N
    2 our N is facing board E
    3 our N is facing board S
    4 our N is facing board W
  */

  /*hardcoded thresholds...maybe should be set by calibration*/
  int NWthresh=170;
  int NEthresh=170;
  int SWthresh=170;
  int SEthresh=170;

  int NWcolor, NEcolor, SWcolor, SEcolor; /*1=black, 0=white*/
  int NWreading=0;
  int NEreading=0;
  int SWreading=0;
  int SEreading=0;

  int counter=0;  
  int tempavg=0;

  while (counter<1)
    {
      NWreading=NWreading+sensor_color_NW();
      NEreading=NEreading+sensor_color_NE();
      SWreading=SWreading+sensor_color_SW();
      SEreading=SEreading+sensor_color_SE();
      counter=counter+1;
    }

  /*find averages*/
  if (counter!=0)
    {
      NWreading=NWreading/counter;
      NEreading=NEreading/counter;
      SWreading=SWreading/counter;
      SEreading=SEreading/counter; 
    }
  else
    {
      printf("Error. Color Sensor avg counter=0\n");
      orientation=0;
      return orientation;
    }
  


  /*for debugging only....comment out later when done*/
  if (NWreading>NWthresh)
    NWcolor=1;
  else
    NWcolor=0;

  if (NEreading>NEthresh)
    NEcolor=1;
  else
    NEcolor=0;

  if (SWreading>SWthresh)
    SWcolor=1;
  else
    SWcolor=0;

  if (SEreading>SEthresh)
    SEcolor=1;
  else
    SEcolor=0;

  /*end of debugging section*/



  /*code to find the odd one out:
    basically, take averages of three quadrants and see if all values lie near the average*/

  /*doesn't work as well as threshold method */
  /*
  tempavg=(NWreading+NEreading+SEreading)/3;
  if (abs(NWreading-tempavg)<15&&abs(NEreading-tempavg)<15&&abs(SEreading-tempavg)<15)
    orientation=1;
  else
    {
      tempavg=(NWreading+NEreading+SWreading)/3;
      if (abs(NWreading-tempavg)<15&&abs(NEreading-tempavg)<15&&abs(SWreading-tempavg)<15)
	orientation=2;
      else
	{
	  tempavg=(NWreading+SWreading+SEreading)/3;
	  if (abs(NWreading-tempavg)<15&&abs(SWreading-tempavg)<15&&abs(SEreading-tempavg)<15)
	    orientation=3;
	  else
	    {
	      tempavg=(NEreading+SWreading+SEreading)/3;
	      if (abs(NEreading-tempavg)<15&&abs(SWreading-tempavg)<15&&abs(SEreading-tempavg)<15)
		orientation=4;
	      else
		orientation=0;
	    }
	}
    }
  */

  if (NWcolor&&SEcolor)
    {
      /*we're on a predominantly black start region*/
      if (NEcolor==0&&SWcolor==1)
	orientation=3;
      else if (NEcolor==1&&SWcolor==0)
	orientation=1;
    }
  else if (!NWcolor&&!SEcolor)
    {
      /*we're on a predominantly white start region*/
      if (NEcolor==0&&SWcolor==1)
	orientation=1;
      else if (NEcolor==1&&SWcolor==0)
	orientation=3;
    }
  else if (NEcolor&&SWcolor)
    {
      /*we're on a predominantly black start region*/
      if (NWcolor==0&&SEcolor==1)
	orientation=4;
      else if (NWcolor==1&&SEcolor==0)
	orientation=2;
    }
  else if (!NEcolor&&!SWcolor)
    {
      /*we're on a predominantly white start region*/
      if (NWcolor==0&&SEcolor==1)
	orientation=2;
      else if (NWcolor==1&&SEcolor==0)
	orientation=4;
    }
  else
    {
      /*this is an error in color/orientation detection!*/
      orientation=0;
    }


  /*debugging output*/
 
  printf("%d%d              %d%d    facing=%d\n",NWcolor, NEcolor, SWcolor, SEcolor, orientation);
  
  return orientation;
}