/*
Program ID  : WLessComm_NXT.nxc
Create date : 2-28-2012
Author      : CH, Chen (Taiwan)
Description :
Wireless comm between Arduino and NXT
*/

#define MAGICWAND          0x70
#define MAGICWAND_PORT     S1
#define NXTCOLOR_PORT      S3
#define NUM_OF_LEDS         7

/* *******************************************************
 *
 * ******************************************************* */
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);
}
/* *******************************************************
 * 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);
}

/* *******************************************************
 * Fireworks effect of MagicWand
 * ******************************************************* */
void displayFireworks(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, xjj=(NUM_OF_LEDS-1); xii<(NUM_OF_LEDS-1) && xjj>0; xii++, xjj--)
      {
          ledBar = ledON[xii] & ledON[xjj];
          lightLED(ledBar);
          Wait(timeDelay);

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

          ledBar = ledON[xii+1] & ledON[xjj-1];
          lightLED(ledBar);
          Wait(timeDelay*2);
      }
   }
   lightLED(0xFF);
}

/* *******************************************************
 * Display specified number of LEDs
 * ******************************************************* */
void displaylightBar(int xnoLEDs, const long waitMillis)
{
   // Bottom up sequence of LED(D7 to D0)
   // b0111 1111, 0011 1111, 0001 1111, 0000 1111,
   //  0000 0111, 0000 0011, 0000 0001, 0000 0000
   byte ledBarON[]  = {0x7F, 0x3F, 0x1F, 0x0F, 0x07, 0x03, 0x01, 0x00};
   if (xnoLEDs > 7) xnoLEDs = 7;
   lightLED(ledBarON[xnoLEDs]);
   Wait(waitMillis);
}

/* *******************************************************
 * Display Hearbeat effect
 * ******************************************************* */
void displayHeartbeat(int xtimes)
{
   for(int xii=0; xii<xtimes; xii++)
   {
      for(int xjj=1; xjj<=7; xjj++)
      {
         displaylightBar(xjj, 50);
      }
      Wait(500);
      for(int xjj=7; xjj>=0; xjj--)
      {
          displaylightBar(xjj, 50);
      }
   }
}

/* *******************************************************
 * Lighting on the specified color of the NXT Color sensor
 * xcolor : NXT Color sensor type,
 *          SENSOR_TYPE_COLORFULL,  SENSOR_TYPE_COLORBLUE,
 *          SENSOR_TYPE_COLORGREEN, SENSOR_TYPE_COLORRED
 * ******************************************************* */
void displayColor(const byte xcolor, int xtimes)
{
   for(int xii=0; xii<xtimes; xii++)
   {
      SetSensorType(NXTCOLOR_PORT, xcolor);
      Wait(300);
      SetSensorColorNone(NXTCOLOR_PORT);
      Wait(200);
   }
   SetSensorType(NXTCOLOR_PORT, xcolor);
}


/* *******************************************************
 * Execute command
 * ******************************************************* */
void ExecuteCommand(string strCommand)
{

   switch(strCommand)
   {
     case "R" : displayColor(SENSOR_TYPE_COLORRED,   2); break;
     case "G" : displayColor(SENSOR_TYPE_COLORGREEN, 2); break;
     case "B" : displayColor(SENSOR_TYPE_COLORBLUE,  2); break;
     case "K" : displayKnightRider(3); break;
     case "F" : displayFireworks(3);   break;
     case "H" : displayHeartbeat(3);   break;
     default:
            if (strCommand >= "0" && strCommand <= "7" )
            {
               displaylightBar(StrToNum(strCommand), 200);
            }
            break;
   }
}

/* *******************************************************
 * Waiting for command
 * ******************************************************* */
void Go_Rtn()
{
   string strAction;
   byte ACK[] = {'@'} ;
   TextOut(0, LCD_LINE5, "RcvData:        " );
   TextOut(0, LCD_LINE6, " Length:        " );
   while(TRUE)
   {
        TextOut(54, LCD_LINE5, "        " );
        TextOut(54, LCD_LINE6, "        " );
        TextOut( 0, LCD_LINE8, "Listening ...   " );
        displayColor(SENSOR_TYPE_COLORGREEN,2);
        
        strAction = "" ;
        until(RS485DataAvailable()) Wait(1);
        RS485Read(strAction);

        TextOut(54, LCD_LINE5, strAction);
        NumOut (54, LCD_LINE6, StrLen(strAction));
        TextOut(0, LCD_LINE8, "Executing ...   " );
        
        displayColor(SENSOR_TYPE_COLORRED,1);
        
        for(int xii=0; xii<StrLen(strAction); xii++)
        {
           if(SubStr(strAction,xii,1) == "@" ) break;
           else ExecuteCommand(SubStr(strAction,xii,1));
        }
        
        displayColor(SENSOR_TYPE_COLORBLUE,1);
        TextOut( 0, LCD_LINE8, "Reply ACK       " );
        RS485Write(ACK);
        while (RS485SendingData()) Wait(1);
   }
}

/* *******************************************************
 * Initialization
 * ******************************************************* */
void Init_Rtn()
{
   SetSensorLowspeed(MAGICWAND_PORT);
   lightLED(0xFF);
   // Configure Hi-Speed port(port 4)
   UseRS485();
   RS485Uart(HS_BAUD_9600, HS_MODE_8N1);
}

/* *******************************************************
 * Main routine
 * ******************************************************* */
task main()
{
   ClearScreen();
   TextOut(0, LCD_LINE1, "Arduino <-> NXT " );
   TextOut(0, LCD_LINE2, "By XBee Wireless" );
   TextOut(0, LCD_LINE3, " CH Lego (2011) " );
   
   Init_Rtn();
   Go_Rtn();
}