/* rail*/

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

//distance apart variables

int mr = 0;
int rc_b = 0;
int diff_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;

int set_b = 0;

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;
                             // 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);
  sleep(2);
  while(1)                                               // the give up salution
  {
    msleep(1);
    if (rc_b < -3 || rc_b > 3)
    {
      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);
    }
  }
}