/* rail*/

#include <conio.h>
#include <unistd.h>
#include <dsensor.h>
#include <dmotor.h>
#include <time.h>
#include <remote.h>

//distance apart variables

int mr = 0;
int rc_b = 0;
int diff_b = 0;
//setup
int set_b = 0;

//time apart variables

int long time_b = 0;
int speed_b = 0;

//give up variables

int old_b = 400; //not 0 for setup to work
int oldb_1 = 0;
int oldb_2 = 0;

//ir remote variables
int ir = 0;
int show = 0;
int stop = 0;
int long control = 0;
////////////////////////////////////////////////////////////////////////////////
int RemoteEvent(unsigned int etype, unsigned int key) {
  if (etype == LREVT_KEYON) {
    switch (key) {
    case LRKEY_P1:
      show = 1;
      break;
    case LRKEY_P2:
      show = 2;
      break;
      case LRKEY_P3:
      show = 3;
      break;
    case LRKEY_P4:
      show = 4;
      break;
    case LRKEY_P5:
      show = 5;
      break;
    case LRKEY_STOP:
      show = 0;
      break;
    }
  }
}
////////////////////////////////////////////////////////////////////////////////
int routine()
{
  while(1)  {
    if (show > 0);  {
      if (show == 1)  {
        while(mr < 10) ir = (mr + 3), msleep(1);
        ir = 11;
        msleep(20);
        ir = 14;
        msleep(250);
        while(mr > 4) ir = (mr + -3), msleep(1);
        ir = 3;
        msleep(30);
        ir = 0;
        msleep(1300);
      }
      if (show == 2)  {
        while(mr < 10) ir = (mr + 3), msleep(1);
        ir = 11;
        msleep(20);
        ir = 14;
        msleep(350);
        while(mr > 4) ir = (mr + -6), msleep(1);
        ir = 3;
        msleep(40);
        ir = 0;
        msleep(1300);
      }
      if (show == 3)  {
        ir = 24;
        msleep(250);
        ir = -30;
        msleep(50);
        while(mr > 4) ir = (mr + -2), msleep(1);
        ir = 4;
        msleep(30);
        ir = 0;
        msleep(1300);
      }
        if (show == 4)  {
        while(mr < 18) ir = (mr + 3), msleep(1);
        ir = 17;
        msleep(20);
        ir = 20;
        msleep(200);
        ir = -30;
        msleep(25);
        while(mr > 4) ir = (mr + -3), msleep(1);
        ir = 3;
        msleep(40);
        ir = 0;
        msleep(1300);
      }
      if (show == 5)  {
        ir = 30;
        msleep(300);
        ir = - 30;
        msleep(300);
        ir = 30;
        msleep(300);
        ir = - 30;
        msleep(600);
        ir = 30;
        msleep(50);
        while(mr < -4) ir = (mr + 3), msleep(1);
        ir = -3;
        msleep(30);
        ir = 0;
        msleep(900);
      }
     else ir = 0, msleep(1);
    }
  }
}
int rail()
{
  motor_b_speed(150);
  motor_b_dir(rev);
  msleep(100);
  while(ROTATION_2 != set_b)
  {
    set_b = ROTATION_2;
    msleep(250);
  }
  ds_rotation_set(&SENSOR_2,- 35);
  msleep(10);
  while(1)
  {
    mr = ROTATION_2;
    rc_b = (ROTATION_1 + ir);
                             // While marble rail and controller are not equal
    if (rc_b != mr && rc_b != old_b)
    {
                            //  Find difference apart if both are pos or neg
                                               //inverse mr and ad them together
      if (mr < 1 && rc_b < 1 || mr > -1 && rc_b > -1)diff_b = mr * -1 + rc_b;
      else// Find difference when one is positive  & one is negative
      {
        if(mr < 0)diff_b = mr * -1 + rc_b;         // Inverse mr then add to rc
        else diff_b = rc_b * -1 + mr;           // If marble rail is positive
      }                                        //inverse rc then add to mr
                                         //make sure we get the absolute value
      if(diff_b < 0)diff_b *= -1;
                                             //time apart + distanc = speed
      speed_b = ((get_system_up_time() - time_b) * 1 / 10) + (diff_b * 25);

      if(mr < rc_b)                        // go forword
      {                                  //
        if(speed_b > 255) speed_b = 255;
        motor_b_speed(speed_b);
        motor_b_dir(fwd);
      }                                  //do same as above but reverse dir
      else
      {
          speed_b =((speed_b * 6) / 10);    //less power for down
          if(speed_b > 255) speed_b = 255;
          motor_b_speed(speed_b);
          motor_b_dir(rev);
      }
    }                                 //marble rail & controller are the same
    else
    {
      motor_b_speed(255);
      motor_b_dir(brake);
      time_b = get_system_up_time();                        //set time_b to curent time
    }                                               //keep contoller in range
    if(ROTATION_1 > 33)ds_rotation_set(&SENSOR_1,33);
    if(ROTATION_1 < -33)ds_rotation_set(&SENSOR_1,-33);
  }
}
int main()
{
  /* turn it on */
  ds_active(&SENSOR_1);
  ds_rotation_on(&SENSOR_1);
  ds_active(&SENSOR_2);
  ds_rotation_on(&SENSOR_2);
  
  /* calibrate it to 0 */
  ds_rotation_set(&SENSOR_1,0);
  ds_rotation_set(&SENSOR_2,0);
  msleep(10);
  execi(&rail, 0, NULL, 1, DEFAULT_STACK_SIZE);
  execi(&routine, 0, NULL, 1, DEFAULT_STACK_SIZE);
  lr_init();
  lr_set_handler(RemoteEvent);
  sleep(2);
  while(1)                                               // the give up salution
  {
    //lcd_int(mr);
    msleep(1);
    if ((rc_b < -3 || rc_b > 3) && (show == 0))
    {
      oldb_1 = rc_b;
      msleep(150);
      //lcd_int(mr);
      oldb_2 = rc_b;
      msleep(1);
      if (rc_b == oldb_1 && rc_b == oldb_2)old_b = rc_b;
      else old_b = (rc_b + 100);
    }
  }
}