import stamp.core.*;
import stamp.util.text.*;

public class StargazerFinal {

  static Uart myUart = new Uart(Uart.dirReceive,CPU.pin2,Uart.invert,Uart.speed9600,Uart.stop1);
  static int a,i;
  static int x = 0;
  static int y = 0;

  static int finx = 0;
  static int finy = 0;
  static int mulr = 0;
  static int mulx = 0;
  static int muly = 0;

  static int r = 15;
  static char buf[] = new char[16];
  static int state = 0;
  static int index = 0;
  static int value[] = new int[1];
  static PWM pwmR = new PWM(CPU.pin12); // create right servo
  static PWM pwmL = new PWM(CPU.pin13); // create left servo

  static int MapID, Angle, Boebot3x, Boebot3y, Height;

  static void main() {
    i = 0;
    while (true) {
      switch (state) {
        case 0: //wait for ~
                a = myUart.receiveByte() & 255;
                if (a == '~') state = 1;
                break;
        case 1: //wait for ^
                a = myUart.receiveByte() & 255;
                if (a == '^') state = 2;
                else state = 0;
                break;
        case 2: //wait for I
                a = myUart.receiveByte() & 255;
                if (a == 'I') {
                  index = 0;
                  state = 3;
                }
                else state = 0;
                break;
        case 3: //wait for |, storing +,-, Landmark Map ID
                a = myUart.receiveByte() & 255;
                if (a != '|') buf[index++] = (char)a;
                else {
                  buf[index] = 0;
                  Format.sscanf(buf,"%d",value); //convert signed number in buf to binary value
                  MapID = value[0];
                  Format.printf("Landmark Map ID = %d :", MapID);
                  index = 0;
                  state = 4;
                }
                break;
        case 4: //wait for |, storing +,-,Angle
                a = myUart.receiveByte() & 255;
                if (a != '|') buf[index++] = (char)a;
                else {
                  buf[index] = 0;
                  Format.sscanf(buf,"%d",value); //convert signed number in buf to binary value
                  Angle = value[0];
                  Format.printf("Angle = %d :",Angle);

/////////////////////////////////////////////////////////
                if ((Angle == 90)||(Angle == 92)||(Angle == 88)||(Angle == 86)||(Angle == 94)) {
                  //turn();
                   System.out.println("if Loop");
                   stopturn();

                  // newsub();
                  /*pwmR.update ( 173, 2304 ) ;
                  pwmL.update ( 173, 2304 ) ;
                  pwmR.start () ;
                  pwmL.start () ;
                  CPU.delay(100);
                  pwmR.stop () ;
                  pwmL.stop () ;    */
                  //return;
                                   }

                else {
                  //stopturn();

                  System.out.println("Else Loop");
                   turn();
                 /* pwmR.update ( 170, 2304 ) ;
                  pwmL.update ( 170, 2304 ) ;
                  pwmR.start () ;
                  pwmL.start () ;
                  CPU.delay(100);
                  pwmR.stop () ;
                  pwmL.stop () ;  */
                      }

//////////////////////////////////////////////////////////
                  index = 0;
                  state = 5;
                }
                break;

        case 5: //wait for |, storing +,-,X-Coordinates
                a = myUart.receiveByte() & 255;
                if (a != '|') buf[index++] = (char)a;
                else {
                  buf[index] = 0;
                  Format.sscanf(buf,"%d",value); //convert signed number in buf to binary value
                  Boebot3x = value[0];
                  Format.printf("Boebot3x = %d :",Boebot3x);
                  index = 0;
                  state = 6;
                }
                break;

        case 6: //wait for |, storing +,-,Y-Coordinates
                a = myUart.receiveByte() & 255;
                if (a != '|') buf[index++] = (char)a;
                else {
                  buf[index] = 0;
                  Format.sscanf(buf,"%d",value); //convert signed number in buf to binary value
                  Boebot3y = value[0];
                  Format.printf("Boebot3y = %d :",Boebot3y);

                 /* if ((Boebot3x - x > 0) && (Boebot3y - y > 0))
                  {//  ++ scenario
                    System.out.println ("++");
                  }
                  else if ((Boebot3x - x > 0) && (Boebot3y - y < 0))
                  {//  +- scenario
                    System.out.println ("+-");
                  }
                  else if ((Boebot3x - x < 0) && (Boebot3y - y > 0))
                  {//  -+ scenario
                    System.out.println ("--");
                  }
                  else if ((Boebot3x - x < 0) && (Boebot3y - y < 0))
                  {//  -- scenario
                    System.out.println ("--");
                  }    */

                  //moveStraight();

                  finx = Boebot3x - x;
                  mulx = finx*finx;
                  finy = Boebot3y - y;
                  muly = finy*finy;
                  mulr = r*r;

                  if (mulr + muly - mulr <= 0)
                  {
                    System.out.println ("reach dest");
                    continue;
                    }
                  else
                  {
                  pwmR.update ( 165, 2304 ) ;
                  pwmL.update ( 196, 2304 ) ;
                  pwmR.start () ;
                  pwmL.start () ;
                  CPU.delay(100);
                  pwmR.stop () ;
                  pwmL.stop () ;
                  System.out.println ("Straight");
                  }
                  index = 0;
                  state = 7;
                }
                break;


/*        case 7: //wait for |, storing +,-,Height
                a = myUart.receiveByte() & 255;
                if (a != '|') buf[index++] = (char)a;
                else {
                  buf[index] = 0;
                  Format.sscanf(buf,"%d",value); //convert signed number in buf to binary value
                  Height = value[0];
                  Format.printf("Height = %d :",Height);
                  index = 0;
                  state = 0;
                }
                break;
*/

          case 7: //storing +,-,Height
                a = myUart.receiveByte() & 255;
                if ((a == '-') || (a == '+') || ((a >= '0') && (a <= '9'))) buf[index++] = (char)a;
                else {
                  buf[index] = 0;
                  Format.sscanf(buf,"%d",value); //convert signed number in buf to binary value
                  Height = value[0];
                  Format.printf("Height = %d\n",Height);
                  index = 0;
                  state = 0;
                }
                break;

      }
    }
  }

//////////////////////////////////////////////////////////

  public static void stopturn (){
                  System.out.println("Stop Turn");

                  pwmR.update ( 173, 2304 ) ;
                  pwmL.update ( 173, 2304 ) ;
                  pwmR.start () ;
                  pwmL.start () ;
                  CPU.delay(10000);
                  pwmR.stop () ;
                  pwmL.stop () ;
                               }
  public static void turn (){
                  System.out.println("Turn");

                  pwmR.update ( 170, 2304 ) ;
                  pwmL.update ( 170, 2304 ) ;
                  pwmR.start () ;
                  pwmL.start () ;
                  CPU.delay(100);
                  pwmR.stop () ;
                  pwmL.stop () ;
                      }
 /* public static void moveStraight (){
                  System.out.println("Move Straight");
                  pwmR.update ( 170, 2304 ) ;
                  pwmL.update ( 175, 2304 ) ;
                  pwmR.start () ;
                  pwmL.start () ;
                  CPU.delay(100);
                  pwmR.stop () ;
                  pwmL.stop () ;

  }     */
//////////////////////////////////////////////////////////


}