/* base*/
#include <conio.h>
#include <unistd.h>
#include <dsensor.h>
#include <dmotor.h>
#include <time.h>
#include <dbutton.h>
#include <remote.h>

int rc_a = 0;
int rc_c = 0;
int base_a = 0;
int base_c = 0;
int old_a = 400;
int old_c = 400;
int set_dir = 0;

int base_left()
{
  int set_a = 0;
  int diff_a = 0;
  int long time_a = 0;
  int speed_a = 0;
  motor_a_speed(255);
  motor_a_dir(rev);
  set_a = ROTATION_1;
  msleep(500);
  while(ROTATION_1 != set_a)
  {
    set_a = ROTATION_1;
    msleep(500);
  }
  msleep(250);
  ds_rotation_set(&SENSOR_1,- 45);
  base_a = ROTATION_1;
  while (base_c > - 45)msleep(1);
  msleep(10);              //otherwise cause infinate loop with base right setup
  while(1)
  {
    msleep(1);
    base_a = (ROTATION_1);      //While marble rail and controller are not equal
    if (rc_a != base_a && rc_a != old_a)
    {                             //Find difference apart if both are pos or neg
      if (base_a < 1 && rc_a < 1 || base_a > -1 && rc_a > -1)
      {                                        //inverse mr and ad them together
        diff_a = base_a * -1 + rc_a;
      }                //Find difference when one is positive  & one is negative
      else
      {                                              //Inverse mr then add to rc
        if(base_a < 0)diff_a = base_a * -1 + rc_a;
                          //If marble rail is positive inverse rc then add to mr
        else diff_a = rc_a * -1 + base_a;
      }                                    //make sure we get the absolute value
      if(diff_a < 0)diff_a *= -1;                 //time apart + distanc = speed
      speed_a = ((get_system_up_time() - time_a) * 1 / 10) + (diff_a * 30);//////////////////

      if(base_a < rc_a)                                    // go forword or down
      {                                      //rubber band & gravity fix
          if (base_a > 0)speed_a =(((speed_a * base_a) / 60)+ (speed_a *2/ 10));
                                                //increase power with stetch
          else speed_a = ((speed_a * 2) /  10);
          if(speed_a > 255)speed_a = 255;
          motor_a_speed(speed_a);
          motor_a_dir(fwd);
          set_dir = 1;                                                   //right
      }                                 //do same as above but up and full power
      else
      {
        if(speed_a > 255)speed_a = 255;
        motor_a_speed(speed_a);
        motor_a_dir(rev);
        set_dir = 2;                                                      //left
      }
    }
    else                                 //marble rail & controller are the same
    {
      set_dir = 3;                                                      //random
      if (base_a > -21)
      {
        motor_a_speed(speed_a);
        motor_a_dir(brake);
      }
      else                        //rubber band not stretched far enough to hold
      {
        motor_a_speed(30);
        motor_a_dir(rev);
      }
      time_a = get_system_up_time();                      //set t to curent time
    }
  }
}
////////////////////////////////////////////////////////////////////////////////
int base_right()
{
  int set_c = 0;
  int diff_c = 0;
  int long time_c = 0;
  int speed_c = 0;
  motor_c_speed(255);
  motor_c_dir(rev);
  set_c = ROTATION_3;
  msleep(500);
  while(ROTATION_3 != set_c)
  {
    set_c = ROTATION_3;
    msleep(500);
  }
  msleep(250);
  ds_rotation_set(&SENSOR_3,-45);
  base_c = ROTATION_3;
  while (base_a > -45)msleep(1);
  msleep(10);
  while(1)
  {
    msleep(1);
    base_c = ROTATION_3;
    //rc_c = (ROTATION_2);
    if (rc_c != base_c && rc_c != old_c)
    {
      if (base_c < 1 && rc_c < 1 || base_c > -1 && rc_c > -1)
      {
        diff_c = base_c * -1 + rc_c;
      }
      else
      {
        if(base_c < 0)diff_c = base_c * -1 + rc_c;
        else diff_c = rc_c * -1 + base_c;
      }
      if(diff_c < 0) diff_c *= -1;
      speed_c = ((get_system_up_time() - time_c) * 1 / 10) + (diff_c * 30);  /////////////////
      if(base_c < rc_c)
      {
        if (base_c > 0)speed_c =(((speed_c * base_c) / 60) + (speed_c *2/ 10));
        else speed_c = ((speed_c * 2) / 10);
        if(speed_c > 255)speed_c = 255;
        motor_c_speed(speed_c);
        motor_c_dir(fwd);
        set_dir = 2;                                                      //left
      }
      else
      {
        if(speed_c > 255) speed_c = 255;
        motor_c_speed(speed_c);
        motor_c_dir(rev);
        set_dir = 1;
      }
    }
    else
    {
      set_dir = 3;                                                         //random
      if (base_c > -21)
      {
        motor_c_speed(speed_c);
        motor_c_dir(brake);
      }
      else
      {
        motor_c_speed(30);
        motor_c_dir(rev);
      }
      time_c = get_system_up_time();
    }
  }
}
////////////////////////////////////////////////////////////////////////////////
int buttons()
{
  int start_position_change = 0;
  int start_position = 0;
  int press = 0;
  int press_p = 0;
  int lock = 0;

  int mode = 1;
  int mode_change = 0;
  while(1)
  {
    lcd_int(lock);
    if(ROTATION_2 > 51)ds_rotation_set(&SENSOR_2,51);
    if(ROTATION_2 < -45)ds_rotation_set(&SENSOR_2,-45);
    msleep(1);
    if (PRESSED(dbutton(),BUTTON_VIEW))
    {
      if (mode_change == 0)mode *= -1,mode_change = 1;
    }
    else mode_change = 0;
    if( mode < 0)
    {
      if (PRESSED(dbutton(),BUTTON_PROGRAM))
      {
        if (press_p == 0)
        {
          if (set_dir < 2)lock = 1;
          else lock = 2;
          press_p = 1;
          //press = 0;   surprise
        }
        if (lock == 1) rc_c = ROTATION_2,old_a =(rc_a + 100);
        else          rc_a = (ROTATION_2 * -1),old_c =(rc_c + 100);
      }
      else
      {
        press_p = 0;
        if (press == 0)
        {
          if (set_dir < 2)lock = 1;
          else lock = 2;
          press = 1;
        }
        if (lock == 1)
        {
          if (start_position_change == 1)
          {
            start_position = (ROTATION_1 * 2);
            start_position_change = 0;
          }
          if ((ROTATION_2 + start_position) > 51)rc_a = 51;
          else
          {
            if ((ROTATION_2 + start_position) < -45)rc_a = -45;
            else rc_a = (ROTATION_2 + start_position);
          }
          rc_c = ROTATION_2;
        }
        else //if (lock == 2)
        {
          if (start_position_change == 1)
          {
            start_position = (ROTATION_3 *2);
            start_position_change = 0;
          }
          if (((ROTATION_2 * -1) + start_position) > 51)rc_c = 51;
          else
          {
            if (((ROTATION_2 * -1) + start_position) < -45)rc_c = -45;
            else rc_c = ((ROTATION_2 * -1) + start_position);
          }
          rc_a =(ROTATION_2 * -1);
        }
      }
    }
    else
    {
      press = 0;
      start_position_change = 1;
      if (PRESSED(dbutton(),BUTTON_PROGRAM))
      {
        if (press_p == 0)
        {
          if (set_dir < 2)lock = 1;
          else lock = 2;
          press_p = 1;
        }
        if (lock == 1) rc_c = ROTATION_2,old_a =(rc_a + 100);
        else          rc_a = (ROTATION_2 * -1),old_c =(rc_c + 100);
      }
      else
      {
        press_p = 0;
        if(ROTATION_2 > 40)rc_a = -41, rc_c = 41;                     /*ds_rotation_set(&SENSOR_2,39),*/
        else
        {
          if(ROTATION_2 < -40)rc_a = 41, rc_c = -41;                  /*ds_rotation_set(&SENSOR_2,-39),*/
          else rc_a =(ROTATION_2 * -1), rc_c = ROTATION_2;
        }
      }
    }
  }
}
//////////////////////////////////////////////////////////////////////////////
int main()
{
  ds_active(&SENSOR_1);
  ds_rotation_on(&SENSOR_1);
  ds_active(&SENSOR_2);
  ds_rotation_on(&SENSOR_2);
  ds_active(&SENSOR_3);
  ds_rotation_on(&SENSOR_3);
  ds_rotation_set(&SENSOR_1,0);
  ds_rotation_set(&SENSOR_2,0);
  msleep(10);
  //motor_b_dir(brake);
  execi(&base_left, 0, NULL, 1, DEFAULT_STACK_SIZE);
  execi(&base_right, 0, NULL, 1, DEFAULT_STACK_SIZE);
  msleep(2100);
  ds_rotation_set(&SENSOR_3,0);
  execi(&buttons, 0, NULL, 1, DEFAULT_STACK_SIZE);

  while(1)                                               // the give up salution
  {
    int olda_1 = 0;
    int olda_2 = 0;
    int oldc_1 = 0;
    int oldc_2 = 0;
    olda_1 = rc_a;
    oldc_1 = rc_c;
    msleep(200);
    //lcd_int(base_c);
    olda_2 = rc_a;
    oldc_2 = rc_c;
    msleep(1);
    if (rc_a == olda_1 && rc_a == olda_2)old_a = rc_a;
    else old_a = (rc_a + 100);
    if (rc_c == oldc_1 && rc_c == oldc_2)old_c = rc_c;
    else old_c = (rc_c + 100);
  }
}