#include "NXCDefs.h"
#define BT_CONN 1
#define OUT_MBOX 1
#define IN_MBOX 5
#define MAXSPEED 100

  int sp, in, l, r, dsp, b, c, i, j, rsp, lsp, n;
  string s, s1, s2;
 
sub BTCheck(int conn)
{
  if (!BluetoothStatus(conn)==NO_ERR)
   {
     TextOut(5,LCD_LINE2,"Error");
     Wait(1000);
     Stop(true);
   }
}

void line(int sp)
{
        int f = sp + abs(sp / 5);
        int r = sp - abs(sp / 5);
        b = MotorRotationCount(OUT_B)%180;
        c = -MotorRotationCount(OUT_C)%180;
        if (sp != 0)
        {
        if (b - c > 1)
        {
           OnFwd(OUT_B, r);
           OnFwd(OUT_C, -f);
        } else
        if (b - c < -1)
        {
           OnFwd(OUT_B, f);
           OnFwd(OUT_C, -r);
        } else
         if (abs(b - c) <= 1)
           OnFwdSync(OUT_BC, sp, -100 * abs(sp) / sp);
        } else
        {
           Off(OUT_BC);
          // RotateMotor(OUT_B, MAXSPEED / 2, c-b);
          // Off(OUT_B);
        };
};

void nline(int sp)
{
        int f = sp + abs(sp / 5);
        int r = sp - abs(sp / 5);
        b = MotorRotationCount(OUT_B)%180;
        c = MotorRotationCount(OUT_C)%180;
        if (sp != 0)
        {
        if (b - c > 1)
        {
           if (l != 0)
           {
             OnFwd(OUT_B, r);
             OnFwd(OUT_C, f);
           } else
           {
             OnFwd(OUT_B, - r);
             OnFwd(OUT_C, - f);
           };
        } else
        if (b - c < -1)
        {         if (l != 0)
           {
             OnFwd(OUT_B, f);
             OnFwd(OUT_C, r);
           } else
           {
             OnFwd(OUT_B, - f);
             OnFwd(OUT_C, - r);
           };
        } else
         if (abs(b - c) <= 1)
           OnFwdSync(OUT_BC, sp, 0);
        } else
        {
           Off(OUT_BC);
          // RotateMotor(OUT_B, MAXSPEED / 2, c-b);
          // Off(OUT_B);
        };
};

/*
void nline()
{
        b = MotorRotationCount(OUT_B)%360;
        c = -MotorRotationCount(OUT_C)%360;
        if (sp > 0)
        {
           if (l != 0)
           {
             lsp = lsp  + l;
             if (lsp > MAXSPEED) rsp = rsp - l;
             if (abs(lsp) > MAXSPEED) lsp = MAXSPEED * abs(lsp)/lsp;
             if (abs(rsp) > MAXSPEED) rsp = MAXSPEED * abs(rsp)/rsp;
             if (lsp <= MAXSPEED) OnFwdSync(OUT_BC, lsp, 100 * rsp / lsp); else
              OnFwdSync(OUT_BC, MAXSPEED, 100 * rsp / lsp);
           } else
           {
             rsp = rsp  + r;
             if (rsp > MAXSPEED) lsp = lsp - l;
             if (abs(lsp) > MAXSPEED) lsp = MAXSPEED * abs(lsp)/lsp;
             if (abs(rsp) > MAXSPEED) rsp = MAXSPEED * abs(rsp)/rsp;
              if (rsp <= MAXSPEED) OnFwdSync(OUT_BC, rsp, -100 * lsp / rsp); else
              OnFwdSync(OUT_BC, MAXSPEED, -100 * lsp / rsp);
           };
        } else
        if (sp < 0)
        {
           if (l != 0)
           {
             lsp = lsp  + l;
             if (lsp > MAXSPEED) rsp = rsp - l;
             if (abs(lsp) > MAXSPEED) lsp = MAXSPEED * abs(lsp)/lsp;
             if (abs(rsp) > MAXSPEED) rsp = MAXSPEED * abs(rsp)/rsp;
             if (lsp <= MAXSPEED) OnFwdSync(OUT_BC, lsp, 100 * rsp / lsp); else
              OnFwdSync(OUT_BC, MAXSPEED, 100 * rsp / lsp);
           } else
           {
             rsp = rsp  + r;
             if (rsp > MAXSPEED) lsp = lsp - l;
             if (abs(lsp) > MAXSPEED) lsp = MAXSPEED * abs(lsp)/lsp;
             if (abs(rsp) > MAXSPEED) rsp = MAXSPEED * abs(rsp)/rsp;
              if (rsp <= MAXSPEED) OnFwdSync(OUT_BC, rsp, -100 * lsp / rsp); else
              OnFwdSync(OUT_BC, MAXSPEED, -100 * lsp / rsp);
           };
        } else
        {
           Off(OUT_BC);
           RotateMotor(OUT_B, MAXSPEED, c-b);
           Off(OUT_B);
        };
};


task main()
{
  BTCheck(0);
  TextOut(5,LCD_LINE1,"Slave receiving");
  SendResponseNumber(OUT_MBOX,0xFF); //unblock master
  sp = 0;
  n = 0;
  while(true)
  {
    n++;
    if (ReceiveRemoteString(IN_MBOX,true,s) != STAT_MSG_EMPTY_MAILBOX)
    {
      TextOut(0,LCD_LINE3,"              ");
      TextOut(5,LCD_LINE3,s);
      s1 = s;
      SendResponseNumber(OUT_MBOX,0xFF);
    }
    Wait(10); //take breath (optional)
    //sp = 0;
    rsp = 0;
   
    dsp = 0;
    l = 0;
    r = 0;
    if (s1 == "Stop" || s1 == "")
    {
      b = MotorRotationCount(OUT_B)%360;
      c = -MotorRotationCount(OUT_C)%360;
      Off(OUT_BC);
     // RotateMotor(OUT_B, MAXSPEED, c-b);
     // Off(OUT_B);
      sp = 0;
      rsp = 0;
      dsp = 0;
      l = 0;
      r = 0;
    }
    else
    {
    i = 0;
    s2 = "";
      do
      {
        i = i + 1;
        s2 = LeftStr(s1, i);
      } while (Copy(s1, i, 1) != " " && i < StrLen(s1));
      dsp = StrToNum(s2);
      TextOut(0,LCD_LINE2,s2);
      j = i+1;
      i = 0;
      s2 = "";
      do
      {
        i = i + 1;
        s2 = Copy(s1, j, i);
      } while (Copy(s1, i+j, 1) != " " && i < StrLen(s1));
      l = StrToNum(s2);
      TextOut(0,LCD_LINE3,s2);
      j = i+1+j;
      i = 0;
      s2 = "";
      do
      {
        i = i + 1;
        s2 = Copy(s1, j, i);
      } while (Copy(s1, i+j, 1) != " " && i < StrLen(s1));
      r = StrToNum(s2);
      if (l != 0 && r != 0) {if (l>r) r = 0; else l = 0;};
      TextOut(0,LCD_LINE5,"              ");
      NumOut(5,LCD_LINE5,sp / 20);
      sp = sp + dsp;
      if (sp > MAXSPEED * 20) sp = MAXSPEED * 20;
      if (sp < -MAXSPEED * 20) sp = -MAXSPEED * 20;
      lsp = sp; rsp = sp;
      if (l == 0 && r == 0)
        line(sp / 20);
      else
        nline(sp / 20);
     }
   }
}