/* ************************************************************
 * TP-BT-Slave.nxc
 * CH, Chen(Taiwan)
 * 2012.4.25
 * ************************************************************ */
#define MAGICWAND       0x70
#define MAGICWAND_PORT  IN_3
#define NUM_OF_LEDS        7

#define BT_CHANNEL 0

byte powerLevel, lastPowerLevel;
string driveState;
struct vectorCoordinate
{
       int x;
       int y;
};

/* *******************************************************
 * lightLED()
 * ******************************************************* */
void lightLED(byte ledBar)
{
     int nbytes;
     while(I2CStatus(MAGICWAND_PORT, nbytes)==STAT_COMM_PENDING);
     byte WriteBuf[];
     ArrayBuild(WriteBuf, MAGICWAND, ledBar);
     I2CWrite(MAGICWAND_PORT, 0,  WriteBuf);
}

/* *******************************************************
 * displayKnightRider(): Knight Rider effect of MagicWand
 * ******************************************************* */
void displayKnightRider(int xtimes)
{
     // b1111 1110, 1111 1101, 1111 1011, 1111 0111,
     //  1110 1111, 1101 1111, 1011 1111, 0111,1111
     byte ledON[]   = {0xFE, 0xFD, 0xFB, 0xF7, 0xEF, 0xDF, 0xBF, 0x7F};
     int  timeDelay = 40;
     byte ledBar;
     for(int xcnt=0; xcnt<xtimes; xcnt++)
     {
        for(int xii=0; xii<(NUM_OF_LEDS-1); xii++)
        {
           ledBar = ledON[xii];
           lightLED(ledBar);
           Wait(timeDelay);

           ledBar = ledON[xii] & ledON[xii+1];
           lightLED(ledBar);
           Wait(timeDelay);

           ledBar = ledON[xii+1];
           lightLED(ledBar);
           Wait(timeDelay*2);
        }
        for(int xii=(NUM_OF_LEDS-1); xii>0; xii--)
        {
           ledBar = ledON[xii];
           lightLED(ledBar);
           Wait(timeDelay);

           ledBar = ledON[xii] & ledON[xii-1];
           lightLED(ledBar);
           Wait(timeDelay);

           ledBar = ledON[xii-1];
           lightLED(ledBar);
           Wait(timeDelay*2);
        }
     }
     lightLED(0x7F);
}

/* *******************************************************
 * displaylightBar(): Display specified number of LEDs
 * REV: Bottom up sequence of LED(D7 to D0)
 *      b0011 1111, 0001 1111, 0000 1111,
 *       0000 0111, 0000 0011, 0000 0001, 0000 0000
 * FWD: Top down sequence of LED(D0 to D7)
 *      b1111 1110, 1111 1100, 1111 1000, 1111 0000,
 *       1110 0000, 1100 0000, 1000 0000
 * ******************************************************* */
void displaylightBar(int xnoLEDs, const long waitMillis, bool isFWD)
{
     byte ledBarON_REV[] = {0x3F, 0x1F, 0x0F, 0x07, 0x03, 0x01, 0x00};
     byte ledBarON_FWD[] = {0xFE, 0xFC, 0xF8, 0xF0, 0xE0, 0xC0, 0x80};
     if (xnoLEDs > 6) xnoLEDs = 6;
     if(isFWD) lightLED(ledBarON_FWD[xnoLEDs]);
     else      lightLED(ledBarON_REV[xnoLEDs]);
     Wait(waitMillis);
}

/* *******************************************************
 * displayLED_Direction(): Display Hearbeat effect
 * ******************************************************* */
void displayLED_Direction(int xtimes, bool isFWD)
{
     for(int xii=0; xii<xtimes; xii++)
     {
        for(int xjj=0; xjj<7; xjj++)
        {
           displaylightBar(xjj, 50, isFWD);
        }
        Wait(500);
        lightLED(0x7F);
     }
}

/* ************************************************************
 * showCommand () function
 * Draw arrow to display moving direction
 * Corrdinate of 8 arrows
 * (33,44)-(49,44)-(65,44)
 * (33,28)-(49,28)-(65,28)
 * (33,12)-(49,12)-(65,12)
 * ************************************************************ */
 #define ARROW_HEAD_SIZE 5
void showCommand(vectorCoordinate moveVector,
                 string btnCommand, string driveCommand, string steerCommand)
{
     ClearScreen();
     TextOut(0, LCD_LINE1, "Vector[    ,   ]");
     TextOut(0, LCD_LINE2, " Slope[        ]");
     RectOut(31,10,36,36);
     TextOut(69, LCD_LINE3, "DRIVE");
     TextOut(69, LCD_LINE4, ":    ");
     TextOut(69, LCD_LINE5, "POWER");
     TextOut(69, LCD_LINE6, ":    ");
     
     NumOut(42, LCD_LINE1, moveVector.x);
     NumOut(72, LCD_LINE1, moveVector.y);
     
     float abs_slope = (moveVector.y / moveVector.x);
     NumOut(42, LCD_LINE2, abs_slope);
     
     switch(driveState)
     {
       case "P": TextOut(75, LCD_LINE4, "STOP"); break;
       case "U": TextOut(75, LCD_LINE4, " FWD"); break;
       case "D": TextOut(75, LCD_LINE4, " REV"); break;
     }
     NumOut(81, LCD_LINE6, powerLevel);

     switch(driveCommand)
     {
       case "N":
         switch(steerCommand)
         {
           case "N":
             break;
           case "L":
             LineOut(49,28,33,28);
             LineOut(33,28,33 + ARROW_HEAD_SIZE,28 + ARROW_HEAD_SIZE);
             LineOut(33,28,33 + ARROW_HEAD_SIZE,28 - ARROW_HEAD_SIZE);
             TextOut(0, LCD_LINE5, "Left ");
             break;
           case "R":
             LineOut(49,28,65,28);
             LineOut(65,28,65 - ARROW_HEAD_SIZE,28 + ARROW_HEAD_SIZE);
             LineOut(65,28,65 - ARROW_HEAD_SIZE,28 - ARROW_HEAD_SIZE);
             TextOut(0, LCD_LINE5, "Right");
             break;
         }
         break;
       case "U":
         TextOut(0, LCD_LINE4, "Fwd");
         switch(steerCommand)
         {
           case "N":
             LineOut(49,28,49,44);
             LineOut(49,44,49 - ARROW_HEAD_SIZE,44 - ARROW_HEAD_SIZE);
             LineOut(49,44,49 + ARROW_HEAD_SIZE,44 - ARROW_HEAD_SIZE);
             break;
           case "L":
             LineOut(49,28,33,44);
             LineOut(33,44,33 + ARROW_HEAD_SIZE,44);
             LineOut(33,44,33,44 - ARROW_HEAD_SIZE);
             TextOut(0, LCD_LINE5, "Left ");
             break;
           case "R":
             LineOut(49,28,65,44);
             LineOut(65,44,65 - ARROW_HEAD_SIZE,44);
             LineOut(65,44,65,44 - ARROW_HEAD_SIZE);
             TextOut(0, LCD_LINE5, "Right");
             break;
         }
         break;
       case "D":
         TextOut(0, LCD_LINE6, "Rev");
         switch(steerCommand)
         {
           case "N":
             LineOut(49,28,49,12);
             LineOut(49,12,49 - ARROW_HEAD_SIZE,12 + ARROW_HEAD_SIZE);
             LineOut(49,12,49 + ARROW_HEAD_SIZE,12 + ARROW_HEAD_SIZE);
             break;
           case "L":
             LineOut(49,28,33,12);
             LineOut(33,12,33,12 + ARROW_HEAD_SIZE);
             LineOut(33,12,33 + ARROW_HEAD_SIZE,12);
             TextOut(0, LCD_LINE5, "Left ");
             break;
           case "R":
             LineOut(49,28,65,12);
             LineOut(65,12,65,12 + ARROW_HEAD_SIZE);
             LineOut(65,12,65 - ARROW_HEAD_SIZE,12);
             TextOut(0, LCD_LINE5, "Right");
             break;
         }
         break;
     }
}

/* ************************************************************
 * makeMoveCommand () function
 * Parameters:
 *   moveVector : returned from praseCommand()
 * Return:
 *   driveCommand : U/D/N - FWD/REV/NoChang
 *   steerCommand : L/R/N - Left/Right/No steer
 * Slope(y/x):
 * (-0.64)-UL-(-1.28)-UN-(0, 63)-UN-( 1.28)-UR-( 0.64)
 * UL                       |                      UR
 * (-0.32)                  |                 ( 0.32)
 * NL                       |                      NR
 * (-99,0)---------------(0,  0)---------------(99,0)
 * NL                       |                      NR
 * ( 0.32)                  |                 (-0.32)
 * DL                       |                      DR
 * ( 0.64)-DL-( 1.28)-DN-(0,-63)-DN-(-1.28)-DR-(-0.64)
 * ************************************************************ */
 #define CORNER_UP_LIMIT   1.28
 #define CORNER_DOWN_LIMIT 0.32
void makeMoveCommand(vectorCoordinate moveVector,
                     string &driveCommand, string &steerCommand)
{
     if(moveVector.x == 0) // Vertical Up or Down
     {
       if(moveVector.y > 0)      driveCommand = "U";
       else if(moveVector.y < 0) driveCommand = "D";
       return;
     }

     if(moveVector.y == 0) // Horizontal Right or Left
     {
       if(moveVector.x > 0)      steerCommand = "R";
       else if(moveVector.x < 0) steerCommand = "L";
       return;
     }
     
     float abs_slope = moveVector.y / moveVector.x;
     if(abs_slope < 0) abs_slope = -1.00 * abs_slope;
     if(moveVector.y > 0)
     {
       if(abs_slope >= CORNER_UP_LIMIT) driveCommand = "U";
       else if(abs_slope >= CORNER_DOWN_LIMIT)
            {
              driveCommand = "U";
              if(moveVector.x > 0) steerCommand = "R";
              else                 steerCommand = "L";
            }
            else
            {
              if(moveVector.x > 0) steerCommand = "R";
              else                 steerCommand = "L";
            }
     }
       
     if(moveVector.y < 0)
     {
       if(abs_slope >= CORNER_UP_LIMIT) driveCommand = "D";
       else if(abs_slope >= CORNER_DOWN_LIMIT)
            {
              driveCommand = "D";
              if(moveVector.x > 0) steerCommand = "R";
              else                 steerCommand = "L";
            }
            else
            {
              if(moveVector.x > 0) steerCommand = "R";
              else                 steerCommand = "L";
            }
     }
}
/* ************************************************************
 * praseCommand () function
 * Return:
 *   btnCommand: P/H/M/L/X/R - Park/Power level:High/Middle/Low/eXit/Dummy
 *   After calling makeMoveCommand() will get:
 *     driveCommand: U/D/N - FWD/REV/NoChang
 *     steerCommand: L/R/N - Left/Right/No steer
 * ************************************************************ */
bool praseCommand(string remoteCommand,
                  string &btnCommand, string &driveCommand, string &steerCommand)
{
     btnCommand   = "N";
     driveCommand = "N";
     steerCommand = "N";
     vectorCoordinate moveVector;
     moveVector.x = moveVector.y = 0;
     bool isCommand = TRUE;
     if(SubStr(remoteCommand,0,1) == "B")
     {
       switch(SubStr(remoteCommand,1,1))
       {
         case "1": btnCommand = "P"; break;
         case "2": btnCommand = "L"; break;
         case "3": btnCommand = "M"; break;
         case "4": btnCommand = "H"; break;
         case "5": btnCommand = "X"; break;
         case "6":
         case "7":
         case "8": btnCommand = "R"; break;
       }
     }
     else if(SubStr(remoteCommand,0,1) == "V")
          {
            moveVector.x = StrToNum(SubStr(remoteCommand,1,4));
            moveVector.y = StrToNum(SubStr(remoteCommand,5,3));
            if(!((moveVector.x == 0) && (moveVector.y == 0)))
              makeMoveCommand(moveVector, driveCommand, steerCommand);
          }
          else
            isCommand = FALSE;
            
     showCommand(moveVector, btnCommand, driveCommand, steerCommand);
     return isCommand;
}

/* ************************************************************
 * executeCommand () function
 * Parameters:
 *   btnCommand: N-No button pressed, P-Pause, H/M/L-Power level High/Middle/Low
 *               X-eXit, R-Dummy
 *   driveCommand: N-Stop, U-Forward, D-Reverse
 *   steerCommand: N-No,   L-Left, R-Right
 * Update global variable:
 *   driveState:  P-Pause, U-Forward, D-Reverse
 * ************************************************************ */
 #define R_MOTOR         OUT_C
 #define L_MOTOR         OUT_A
 #define ALL_MOTORS      OUT_AC
 #define HIGH_POWER      100
 #define MIDDLE_POWER     75
 #define LOW_POWER        50
 #define STEER_DURATION 1200
void executeCommand(string btnCommand,  string driveCommand, string steerCommand)
{
   if(!(btnCommand == "N"))
   {
     switch(btnCommand)
     {
       case "P":
         driveState = "P";
         Off(ALL_MOTORS);
         break;
       case "H": powerLevel = HIGH_POWER;   break;
       case "M": powerLevel = MIDDLE_POWER; break;
       case "L": powerLevel = LOW_POWER;    break;
       case "X": break;
       case "R": break;
     }
     if(!(powerLevel == lastPowerLevel))
     {
       switch(driveState)
       {
         case "D":
           SetOutput(ALL_MOTORS, PowerField, (-1*powerLevel),
                                 UpdateFlagsField, UF_UPDATE_SPEED);
           break;
         case "U":
           SetOutput(ALL_MOTORS, PowerField, powerLevel,
                                 UpdateFlagsField, UF_UPDATE_SPEED);
           break;
       }
       Wait(1);
       lastPowerLevel = powerLevel;
     }
   }
   else if(driveCommand == "U")
        {
          driveState = "U"; // Driving Forward
          switch(steerCommand)
          {
            case "N":
              OnFwd(ALL_MOTORS, powerLevel); // Straight direction
              break;
            case "R":
              OnFwd(L_MOTOR, powerLevel); // turning right
              Float(R_MOTOR);
              Wait(STEER_DURATION);
              OnFwd(ALL_MOTORS, powerLevel);
              break;
            case "L":              // turning left
              OnFwd(R_MOTOR, powerLevel);
              Float(L_MOTOR);
              Wait(STEER_DURATION);
              OnFwd(ALL_MOTORS, powerLevel);
              break;
          }
        }
        else if(driveCommand == "D")
             {
               driveState = "D"; // Driving backward
               switch(steerCommand)
               {
                 case "N":
                   OnRev(ALL_MOTORS, powerLevel);  // straight backward
                   break;
                 case "R":
                   OnRev(L_MOTOR, powerLevel); // turning back-right
                   Float(R_MOTOR);
                   Wait(STEER_DURATION);
                   OnRev(ALL_MOTORS, powerLevel);
                   break;
                 case "L":              // turning back-left
                   OnRev(R_MOTOR, powerLevel);
                   Float(L_MOTOR);
                   Wait(STEER_DURATION);
                   OnRev(ALL_MOTORS, powerLevel);
                   break;
               }
             }
             else if(driveCommand == "N")
                  {
                    switch(steerCommand)
                    {
                      case "R":
                        switch(driveState)
                        {
                          case "U":
                            OnFwd(L_MOTOR, powerLevel); // turning right
                            Float(R_MOTOR);
                            Wait(STEER_DURATION);
                            OnFwd(ALL_MOTORS, powerLevel);
                            break;
                          case "D":              // turning back-right
                            OnRev(L_MOTOR, powerLevel);
                            Float(R_MOTOR);
                            Wait(STEER_DURATION);
                            OnRev(ALL_MOTORS, powerLevel);
                            break;
                          case "P":
                            break;
                        }
                        break;
                      case "L":
                        switch(driveState)
                        {
                          case "U":              // turning left
                            OnFwd(R_MOTOR, powerLevel);
                            Float(L_MOTOR);
                            Wait(STEER_DURATION);
                            OnFwd(ALL_MOTORS, powerLevel);
                            break;
                          case "D":
                            OnRev(R_MOTOR, powerLevel); // turning back-left
                            Float(L_MOTOR);
                            Wait(STEER_DURATION);
                            OnRev(ALL_MOTORS, powerLevel);
                            break;
                          case "P":
                            break;
                        }
                        break;
                    } // switch(steerCommand)
                  } // if(driveCommand == "N")
   Wait(1000);
}

/* ************************************************************
 * displayLED task
 * ************************************************************ */
 #define DISPLAY_LED_DURATION 500
task displayLED()
{
     while(TRUE)
     {
          switch(driveState)
          {
            case "P":
              displayKnightRider(1);
              break;
            case "U":
              displayLED_Direction(1, TRUE);
              break;
            case "D":
              displayLED_Direction(1, FALSE);
              break;
          }
          Wait(DISPLAY_LED_DURATION);
     }
}

/* ************************************************************
 * Main task
 * ************************************************************ */
task main ()
{
     string remoteCommand, btnCommand="N", driveCommand="N", steerCommand="N";

     ClearScreen();
     if(!(BluetoothStatus(BT_CHANNEL)==NO_ERR))
     {
       TextOut(0, LCD_LINE1, "<<< Warning  >>>");
       TextOut(0, LCD_LINE3, "No connection   ");
       TextOut(0, LCD_LINE4, "with master NXT,");
       TextOut(0, LCD_LINE5, "will quit system");
       TextOut(0, LCD_LINE6, "after 3 seconds!");
       Wait(3000);
       Stop(TRUE);
     }
     else
     {
       PlayTone(440,100);
       Wait(1000);
     }

     SetSensorLowspeed(MAGICWAND_PORT);
     StartTask(displayLED);

     powerLevel = MIDDLE_POWER;
     lastPowerLevel = powerLevel;
     driveState = "P"; // P/U/D - Pause/FWD/REV
     while(TRUE)
     {
          TextOut(0, LCD_LINE8, "Listening ...   ");
          if(ReceiveRemoteString(MAILBOX1,TRUE,remoteCommand)!=STAT_MSG_EMPTY_MAILBOX)
          {
            TextOut(0, LCD_LINE8, "Receiving ...   ");
            if(praseCommand(remoteCommand, btnCommand, driveCommand, steerCommand))
            {
              TextOut(0, LCD_LINE8, "Executing ...   ");
              executeCommand(btnCommand, driveCommand, steerCommand);
            }
          }
          Wait(50);
     }
}