#pragma config(Sensor, S1,     LEFTS,          sensorTouch)
#pragma config(Sensor, S3,     RIGHTS,         sensorTouch)
#pragma config(Sensor, S4,     COLOR,          sensorCOLORFULL)
#pragma config(Motor,  motorB,          CARRIAGE,    tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorA,          PUSHER,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          BALLS,       tmotorNXT, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Sorts colored balls
// UK 2.2.2013

// global variables
int VaunuVali;
int VaunuLukema;
int vauhti = 20;
string teksti;
bool jatkuu = false;
int vari;

void liikkuu(int nopeus)
{
  motor[CARRIAGE] = nopeus;
}

void seis()
{
  motor[CARRIAGE] = 0;
}

task main()
{
  wait10Msec(100);
  // texts on startup
  teksti = "";
  for(int i = 0; i <= 4; i++)
  {
    nxtDisplayTextLine(i, "%s", teksti);
  }
  teksti = "left <- pusher";
  nxtDisplayTextLine(1, "%s", teksti);
  teksti = "right -> ball";
  nxtDisplayTextLine(2, "%s", teksti);
  teksti = "orange -> start";
  nxtDisplayTextLine(3, "%s", teksti);
  // setting ball feeder and pusher in right positions
  while (jatkuu != true) {
    while (nNxtButtonPressed == 1) {
      motor[BALLS] = -10;
    }
    motor[BALLS] = 0;
    while (nNxtButtonPressed == 2) {
      motor[PUSHER] = 10;
    }
    motor[PUSHER] = 0;
    if (nNxtButtonPressed == 3) {
       jatkuu = true;
    }
  }

  // measure the distance between touch sensors (VaunuVali)
  // go first to the left then to the right
  liikkuu(vauhti);
  while (SensorValue(LEFTS) == 0)  {  }
  seis();
  VaunuVali = 0;
  nMotorEncoder[CARRIAGE] = 0;
  liikkuu(-vauhti);
  while (SensorValue(RIGHTS) == 0)  {  }
  seis();
  VaunuVali = abs(nMotorEncoder[CARRIAGE]);

  jatkuu = false;
  while (jatkuu != true) {

    // drop one ball to the pusher
    nMotorEncoder[BALLS] = 0;
    nMotorEncoderTarget[BALLS] = 180;
    motor[BALLS] = -5;
    while (nMotorRunState[BALLS] != runStateIdle) {  }
    motor[BALLS] = 0;
    nMotorEncoder[BALLS] = 0;
    nMotorEncoderTarget[BALLS] = 180;
    motor[BALLS] = 10;
    while (nMotorRunState[BALLS] != runStateIdle) {  }
    motor[BALLS] = 0;

    // carriage to left and push ball slightly forward
    liikkuu(vauhti);
    while (SensorValue(LEFTS) == 0)  {  }
    seis();
    nMotorEncoder[PUSHER] = 0;
    nMotorEncoderTarget[PUSHER] = 70;  // slightly forward
    motor[PUSHER] = 10;
    while (nMotorRunState[PUSHER] != runStateIdle) {  }
    motor[PUSHER] = 0;

    // find the color and go to the right box
    vari = SensorValue(COLOR);

    switch (vari) {
      case BLACKCOLOR:  // black means no ball
          jatkuu = true;  // end sorting
          VaunuLukema = VaunuVali / 2;
          break;
      case BLUECOLOR:
          VaunuLukema = 0;  // blue in first box from left
          break;
      case GREENCOLOR:
          VaunuLukema = VaunuVali / 4;  // green to second box
          break;
      case YELLOWCOLOR:
          VaunuLukema = (VaunuVali * 3) / 4;  // yellow to third box
          break;
      case REDCOLOR:
          VaunuLukema = VaunuVali;  // red to the last box
          break;
      default:
          VaunuLukema = VaunuVali;  // unknown color also to last box
    }

    nMotorEncoder[CARRIAGE] = 0;
    nMotorEncoderTarget[CARRIAGE] = VaunuLukema;
    if (VaunuLukema > 0) {
      liikkuu(-vauhti);
      while (nMotorRunState[CARRIAGE] != runStateIdle) {  }
      seis();
    }

    // drop the ball
    if (jatkuu == false) {
      nMotorEncoder[PUSHER] = 0;
      nMotorEncoderTarget[PUSHER] = 290;
      motor[PUSHER] = 10;
      while (nMotorRunState[PUSHER] != runStateIdle) {  }
      motor[PUSHER] = 0;
      wait10Msec(20);
      liikkuu(-vauhti);
      while (SensorValue(RIGHTS) == 0)  {  }
      seis();
    }
  }

  // exit the program
  PlaySound(soundBeepBeep);
  StopAllTasks();
}