/*Calibration routine: Goes through series of calibration techniques to read the four directions. Eventually, wait for start light, and orient itself. */
/*Functions:
  calibrate                          Main calibration Routine
  calibratestep1 - calibratestep4    4 directions
  calibratestep5 - calibratestep7    Ground lights
  calibratedone                      Final calibration confirmation
  waitfornext                        Pauses until next is pressed
  waitforenter                       pauses until enter is pressed
  whichbutton                        returns which button was pressed: prev, next, enter
*/
  


/* Calibration arrays: Orientation:F=Forward, L=Left, B=Backward, R=Right
   Sensor:     0=Front, 1=Left, 2=Back, 3=Right  */

int F[] = {0, 0, 0, 0};
int L[] = {0, 0, 0, 0};
int B[] = {0, 0, 0, 0};
int R[] = {0, 0, 0, 0};

/* Calibration variables: */
int groundjail = 0;
int groundwhite = 0;
int groundblack = 0;


/****************************
 * Main Calibration Routine *
 ****************************/

void calibrate() {
  printf("\nOrientation: press next\n");
  waitfornext();
  statemachine2();  
  aftercalibrate();
}

void statemachine2() {
  int state = 1;
  
  while(1) {
    if (state == 1)
      state = calibratestep1();
    else if (state == 2)
      state = calibratestep2();
    else if (state == 3)
      state = calibratestep3();
    else if (state == 4)
      state = calibratestep4();
    else if (state == 5)
      state = calibratestep5();
    else if (state == 6)
      state = calibratestep6();
    else if (state == 7)
      state = calibratestep7();
    else if (state == 8)
      state = calibratedone();
    else if (state == 9)
      break;
    else Errors("Bad state!\n");
  }
}

/********************************
 * What to do after calibration *
 ********************************/

int aftercalibrate() {
  /*
  int startlightcutoff

  printf("Waiting: orientation and start\n");
  startlightcutoff = (startlighton + startlightoff)/2;
  while(startsensor > startlightcutoff) {
  printf("current start: %d cutoff: %d\n",startsensor,startlightcutoff);
  msleep(100L);
  }
  printf("Started!\n"); */

  start_machine(startport);
}

/***************************
 * Orientation calibration *
 ***************************/

/*Step 1: Check sensors facing forwards (facing other robot) */
int calibratestep1() {
  int choice;

  printf("\nForwards: now %d, %d, %d, %d\n",F[0],F[1],F[2],F[3]);
  choice = whichbutton();
  if (choice == 1)
    return 8;
  else if (choice == 2)
    return 2;
  else if (choice == 3) {
    printf("Enter to calibrate Forward\n");
    waitforenter();
    F[0]=light0;
    F[1]=light1;
    F[2]=light2;
    F[3]=light3;
    return 1;
  }
  else
      Errors("calibratestep1 :1");
}

/* Step 2: Check sensors facing left (with left side of robot at light) */
int calibratestep2() {
  int choice;

  printf("\nLeft: now %d, %d, %d, %d\n",L[0],L[1],L[2],L[3]);
  choice = whichbutton();
  if (choice == 1)
    return 1;
  else if (choice == 2)
    return 3;
  else if (choice == 3) {
    printf("Enter to calibrate Left\n");
    waitforenter();
    L[0]=light0;
    L[1]=light1;
    L[2]=light2;
    L[3]=light3;
    return 2;
  }
  else
      Errors("calibratestep2 :1");
}

/*Step 3: Check sensors facing backwards (facing light) */
int calibratestep3() {
  int choice;

  printf("\nBack: now %d, %d, %d, %d\n",B[0],B[1],B[2],B[3]);
  choice = whichbutton();
  if (choice == 1)
    return 2;
  else if (choice == 2)
    return 4;
  else if (choice == 3) {
    printf("Enter to calibrate Back\n");
    waitforenter();
    B[0]=light0;
    B[1]=light1;
    B[2]=light2;
    B[3]=light3;
    return 3;
  }
  else Errors("calibratestep3 :2");
}

/* Step 4: Check sensors facing right (right side of robot at light) */
int calibratestep4() {
  int choice;

  printf("\nRight: now %d, %d, %d, %d\n",R[0],R[1],R[2],R[3]);
  choice = whichbutton();
  if (choice == 1)
    return 3;
  else if (choice == 2)
    return 5;
  else if (choice == 3) {
    printf("Enter to calibrate Right\n");
    waitforenter();
    R[0]=light0;
    R[1]=light1;
    R[2]=light2;
    R[3]=light3;
    return 4;
  }
  else
    Errors("calibratestep4 :2");
}


/***********************
 * Check ground lights *
 ***********************/

/* Check JAIL groundlights */
int calibratestep5() {
  int choice;

  printf("\nGround Jail: now %d\n",groundjail);
  choice = whichbutton();
  if (choice == 1)
    return 4;
  else if (choice == 2)
    return 6;
  else if (choice == 3) {
    printf("Enter to calibrate Jail\n");
    waitforenter();
    groundjail = ground0;
    return 5;
  }
  else
    Errors("calibratestep5 :2");
}

/* Check White groundlights */
int calibratestep6() {
  int choice;

  printf("Ground White: now %d\n",groundwhite);
  choice = whichbutton();
  if (choice == 1)
    return 5;
  else if (choice == 2)
    return 7;
  else if (choice == 3) {
    printf("Enter to calibrate White\n");
    waitforenter();
    groundwhite = ground0;
    return 6;
  }
  else
    Errors("calibratestep6 :2");
}

/* Check Black groundlights */
int calibratestep7() {
  int choice;

  printf("Ground black: now %d\n",groundblack);
  choice = whichbutton();
  if (choice == 1)
    return 6;
  else if (choice == 2)
    return 8;
  else if (choice == 3) {
    printf("Enter to calibrate Black\n");
    waitforenter();
    groundblack = ground0;
    return 7;
  }
  else
    Errors("calibratestep7 :2");
}


/****************************
 * Calibrate Starting Light *
 ****************************/
/*
\\Starting Light OFF
void calibrate8() {
  int choice;

  printf("Starting light off: now %d",startlightoff);
  choice = whichbutton();
  if (choice == 1)
    calibratestep7();
  else if (choice == 2)
    calibratestep9();
  else if (choice == 3) {
    printf("Enter to set Start light off\n");
    waitforenter();
    startlightoff = startsensor0;
    calibratestep8();
  }
  else
    Errors("calibratestep8 :2");
}

\\Starting Light ON
void calibrate9() {
  int choice;

  printf("Starting light on: now %d",startlighton);
  choice = whichbutton();
  if (choice == 1)
    calibratestep8();
  else if (choice == 2)
    calibratedone();
  else if (choice == 3) {
    printf("Enter to set Start light on\n");
    waitforenter();
    startlighton = startsensor0;
    calibratestep9();
  }
  else
    Errors("calibratestep9 :2");
}

*/
/********************
 * Routine for Done *
 ********************/

int calibratedone() {
  int choice;

  printf("Private calib done. Enter to exit\n");
  choice = whichbutton();
  if (choice == 1)
    return 7;
  else if (choice == 2)
    return 1;
  else if (choice == 3) {
    return 9;
  }
  else
    Errors("calibratedone :2");
}