#pragma config(Sensor, in1,    up_switch,           sensorTouch)
#pragma config(Sensor, in2,    down_switch,         sensorTouch)
#pragma config(Sensor, in3,    head_bumper1,        sensorTouch)
#pragma config(Sensor, in4,    head_bumper2,        sensorTouch)
#pragma config(Sensor, in5,    knock_bumper,        sensorTouch)
#pragma config(Sensor, in6,    Sent,                sensorDigitalOut)
#pragma config(Sensor, in7,    Received,            sensorDigitalIn)
#pragma config(Sensor, in8,    number,              sensorDigitalOut)
#pragma config(Sensor, in9,    LED1,                sensorDigitalOut) //RED
#pragma config(Sensor, in10,   LED2,                sensorDigitalOut) //YELLOW
#pragma config(Sensor, in12,   Confirm,             sensorTouch)
#pragma config(Sensor, in13,   input1,              sensorTouch)
#pragma config(Sensor, in14,   input2,              sensorTouch)
#pragma config(Motor,  port2,           left,                 tmotorNormal)
#pragma config(Motor,  port3,           right,                tmotorNormal)
#pragma config(Motor,  port4,           arm,                  tmotorNormal)
#pragma config(Motor,  port5,           tail,                 tmotorNormal)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


int database1[35]; //pigeon hole
int database2[16]; //room
int size_data1;
int size_data2;


void setup();
void wait();
void read();
void rebuild();
void navigate_to_box();


task main()
{
   int mode = 1;
   //int posture = 1;
   size_data1 = 0;
   size_data2 = 0;

   mode = 1;

   //waiting to be awakened
   while(true)
   {
      setup(); //pre set the robot

      while(mode == 1)
      {
        wait();
        if(SensorValue[Confirm] == 1) //call the robot to be ready to work
        {
           mode = 2;
        }
      }

      read();
      rebuild();

      while(mode == 2)
      {
         SensorValue[LED1] = 0;
         SensorValue[LED2] = 0;

         if(SensorValue[Confirm] == 1)
           mode = 3;
      }

      motor[left] = -127;
     motor[right] = 127;
     wait1Msec(0); //calibrate , from p1 to p2

     if(size_data1 > 0)//need to deliver documents to pigeon hole
      {
         //get to starting point of pigeon hole, need to configure time
         navigate_to_box();
      }

   }


}


void setup()
{

   //idle condition
   //need some more auto correction as noise is generated when power is turned on
   motor[arm] = 0;
   motor[left] = 0;
   motor[right] = 0;
   SensorValue[LED1] = 0;
   SensorValue[LED2] = 0;

   for(int i = 0; i < 100; i++)
   {
      database1[i] = 0;
      database2[i] = 0;
   }
}


void wait()
{
   SensorValue[LED1] = 1;
   SensorValue[LED2] = 1;
   motor[tail] = 80;
   wait1Msec(200);
   motor[tail] = -80;
   wait1Msec(400);
   motor[tail] = 80;
   wait1Msec(200);
   return;
}


void read()
{
   bool done1 = false;
   bool done2 = false;
   int data1 = 0;
   int data2 = 0;

   //read pigeon hole no.
   while(!done1)
   {
      SensorValue[LED1] = 1;
     SensorValue[LED2] = 0;

      if(SensorValue[input1] == 1)
      {
         data1 += 1;
         SensorValue[LED2] = 1;
         wait1Msec(500);
      }

      if(SensorValue[input2] == 1)
      {
         data2 += 1;
         SensorValue[LED2] = 1;
         wait1Msec(500);
      }

      if(SensorValue[Confirm] == 1)
      {
         bool end = false;
         SensorValue[LED2] = 1;
         wait1Msec(300);

         ClearTimer(T1);
         while(time1[T1] < 400)
         {
            if(SensorValue[Confirm] == 1) // input of pigeon hole(s) no. is finished
            {
               done1 = true;
               end = true;
               SensorValue[LED2] = 1;
               wait1Msec(400);
            }
         }
         if(!end) //store input
         {
            database1[size_data1] = data1*10 + data2;
            size_data1 += 1;
            data1 = 0;
            data2 = 0;
            wait1Msec(500);
         }
      }
   }

   data1 = 0;
   data2 = 0;

   //read room no.
   while(!done2)
   {
      SensorValue[LED1] = 0;
     SensorValue[LED2] = 1;

     if(SensorValue[input1] == 1)
      {
         data1 += 1;
         SensorValue[LED1] = 1;
         wait1Msec(500);
      }

      if(SensorValue[input2] == 1)
      {
         data2 += 1;
         SensorValue[LED1] = 1;
         wait1Msec(500);
      }

      if(SensorValue[Confirm] == 1)
      {
         bool end = false;
         SensorValue[LED1] = 1;
         ClearTimer(T1);
         while(time1[T1] < 800)
         {
            if(SensorValue[Confirm] == 1)// input of staff room no. is finished
            {
               done2 = true;
               end = true;
               SensorValue[LED1] = 1;
               wait1Msec(400);
            }
         }
         if(!end) //store input
         {
            database2[size_data2] = data1*10 + data2;
            size_data2 += 1;
            data1 = 0;
            data2 = 0;
            wait1Msec(500);
         }
      }
   }
}


//re-make the rundown/route, re-arrange room no, box no in database, quite difficult to do
void rebuild()
{
   int data1[35];
   int data2[16];

   //initial
   for(int i = 0; i < 35; i++)
   {
      data1[i] = 0;
   }
   for(int i = 0; i < 16; i++)
   {
      data2[i] = 0;
   }

   //make temporary record for pigeon hole
   for(int i = size_data1 - 1; i >= 0; i--)
   {
      int j = database1[i]/6;
      data1[j*7] = 1;
      data1[database1[i] + j + 1] = 1;
   }


   //make temporary record for staff room
   for(int i = size_data2 - 1; i >= 0; i--)
   {
      data2[database2[i]-1] = 1;
   }

   //update record
   for(int i = 0; i < 35; i++)
   {
      database1[i] = data1[i];
   }
   for(int i = 0; i < 35; i++)
   {
      database2[i] = data2[i];
   }
}


void navigate_to_box()
{
   //navigation
   motor[left] = 127;
   motor[right] = -127;
   wait1Msec(0); //calibrate, from p2 to BOX P3

   for(int i = 0; i < 5; i++)
   {
      motor[left] = 127;
     motor[right] = -127;
     wait1Msec(0); //calibrate for each column

     motor[left] = 0;
     motor[right] = 0;

     if(database1[i*7] == 1)
     {
        //communicate(0,false,i);
        //communicate(2,false,i);
        //communicate(1,false,i);
        //communicate(2,false,i);
     }
   }

   //turn 2 x 90
   //change_posture(false);
   //change_posture(false);

   //move from p3 to p2
   motor[left] = 127;
   motor[right] = -127;
   wait1Msec(0); //calibrate

   //move back to starting position
}