/**

 * from 992 MRLcomm version... using an uno...
 *I think we are working now.......
 * upload this to your arduino, open terminal(hyperterminal) and tap out the below.          

$MRTST,6,0,6,

$MRTST,6,1,9,

$MRTST,7,0,0,

$MRTST,7,0,180,

$MRTST,7,1,0,

$MRTST,7,1,180,

two servos 1 on pin6, 1 on pin7

 * Edited to allow Direct Serial, MRL still works normal, Direct Serial resonds to proper
 * ascii input with checksum
 * paste $MRCMD,000,011,001*48 or $MRTST,0,11,1, (needs no checksum)
 * TST Requires following comma to register last bit...
 *
* @author greg (at) myrobotlab.org
*
* This file is part of MyRobotLab.
*
* MyRobotLab is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 2 of the License, or
* (at your option) any later version (subject to the "Classpath" exception
* as provided in the LICENSE.txt file that accompanied this code).
*
* MyRobotLab is distributed in the hope that it will be useful or fun,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* All libraries in thirdParty bundle are subject to their own license
* requirements - please refer to http://myrobotlab.org/libraries for
* details.
*
* Enjoy !
*
* arduinoSerial
* -----------------
* Purpose: translate serial commands into Arduino language commands,
* mostly relating to IO.  This would allow a computer to easily take
* advantage of Arduino's great IO capabilities, while cpu number crunching
* could be done on the computer (e.g. Video processing)
*
* Created 2 Februrary 2009
*
* http://myrobotlab.org
*
* TODO - create analogSensitivity (delta) & analogGain (scalar)
* References :
*      http://www.arduino.cc/en/Reference/Constants
*
*/

#include <Servo.h>

#define DIGITAL_WRITE        0
#define DIGITAL_VALUE        1
#define ANALOG_WRITE         2
#define ANALOG_VALUE         3
#define PINMODE              4
#define PULSE_IN             5
#define SERVO_ATTACH         6
#define SERVO_WRITE          7
#define SERVO_SET_MAX_PULSE  8
#define SERVO_DETACH         9
#define SET_PWM_FREQUENCY    11
#define SET_SERVO_SPEED           12
#define ANALOG_READ_POLLING_START  13
#define ANALOG_READ_POLLING_STOP  14
#define DIGITAL_READ_POLLING_START  15
#define DIGITAL_READ_POLLING_STOP  16
#define SET_ANALOG_TRIGGER               17
#define REMOVE_ANALOG_TRIGGER            18
#define SET_DIGITAL_TRIGGER              19
#define REMOVE_DIGITAL_TRIGGER           20
#define DIGITAL_DEBOUNCE_ON              21
#define DIGITAL_DEBOUNCE_OFF             22
#define DIGITAL_TRIGGER_ONLY_ON          23
#define DIGITAL_TRIGGER_ONLY_OFF         24
#define SET_SERIAL_RATE            25

#define COMMUNICATION_RESET    252
#define SOFT_RESET      253
#define SERIAL_ERROR           254
#define NOP  255

#define MAGIC_NUMBER    170 // 10101010

// pin services
#define POLLING_MASK 1
#define TRIGGER_MASK 2
// TODO #define SERVO_SWEEP

// --VENDOR DEFINE SECTION BEGIN--
// --VENDOR DEFINE SECTION END--

// -- FIXME - modified by board type BEGIN --
#define ANALOG_PIN_COUNT 4
#define DIGITAL_PIN_COUNT 13
// -- FIXME - modified by board type END --

long debounceDelay = 50; // in ms
long lastDebounceTime[DIGITAL_PIN_COUNT];

Servo servos[MAX_SERVOS];
int servoSpeed[MAX_SERVOS];    // 0 - 100 corresponding to the 0.0 - 1.0 Servo.setSpeed - not a float at this point
int servoTargetPosition[MAX_SERVOS];  // when using a fractional speed - servo's must remember their end destination
int servoCurrentPosition[MAX_SERVOS]; // when using a fractional speed - servo's must remember their end destination
int movingServos[MAX_SERVOS];    // array of servos currently moving at some fractional speed
int movingServosCount = 0;            // number of servo's currently moving at fractional speed

unsigned long loopCount     = 0;
int byteCount               = 0;
unsigned char newByte   = 0;
unsigned char ioCommand[4];  // most io fns can cleanly be done with a 4 byte code
int readValue;

int digitalReadPin[DIGITAL_PIN_COUNT];        // array of pins to read from
int digitalReadPollingPinCount = 0;           // number of pins currently reading
int lastDigitalInputValue[DIGITAL_PIN_COUNT]; // array of last input values
int digitalPinService[DIGITAL_PIN_COUNT];     // the services this pin is involved in
bool digitalTriggerOnly = false;      // send data back only if its different

int analogReadPin[ANALOG_PIN_COUNT];          // array of pins to read from
int analogReadPollingPinCount = 0;            // number of pins currently reading
int lastAnalogInputValue[ANALOG_PIN_COUNT];   // array of last input values
int analogPinService[ANALOG_PIN_COUNT];       // the services this pin is involved in
bool analogTriggerOnly = false;         // send data back only if its different

unsigned long retULValue;

unsigned int errorCount = 0;

// MRL DirectSerial Protocol
//int newByte = 0;

int inputgroup[10] = {
  0,0,0};
char inputData[300] = "";
char Checksum[3] = "  ";
char Cmd[4] = "   ";
int group = 0;
int group1 =0;
int stat = 0;
int cnt = 0;
int subSection[45];
char iochar[] ="";
int byteMode = 0;
const char MrlID[4] = "$MR";
int calcChecksum ;
int XOR;
int check = 0;
int ChkMatch= 0;
int stopXOR = 0;
int ValidateChecksum = 0;
char hx[5] = "0x00";
char buffer[5] = "0x00";
char ipdatabuff[5] = "0000";
unsigned char nmeaChk;
int PrintVariables = 0;
int ProceedAllowed =0;
int tstMode=0;
int Nogood = 0;
long int li1= 0;
long int li2= 0;
char *pEnd;
int ReturnMode = 0; // when return is required, use to return true or false
int Reset = 0; // when return is required, use to reset variables.
// End MRL DirectSerial definitions

void PrintVariablesList () {

  if (PrintVariables == 1){ //reset variables
    // variables
    Serial.println("============================Variables=================================");
    Serial.print("cnt");
    Serial.println(cnt);
    Serial.print("inputData[i]=");
    Serial.println(inputData);
    Serial.print("group = ");
    Serial.println(group);
    Serial.print("group1 =");
    Serial.println(group1);
    Serial.print("stat=");
    Serial.println(stat);
    Serial.print("check=");
    Serial.println(check);
    Serial.print("calcChecksum=");
    Serial.println(calcChecksum);
    Serial.print("Checksum[] = ");
    Serial.println(Checksum);
    Serial.print("hx[] = ");
    Serial.println(hx);
    Serial.print("nmeaChk =");
    Serial.println(nmeaChk);
    Serial.print("stopXOR = ");
    Serial.println(stopXOR);
    Serial.print("byteCount=");
    Serial.println(byteCount);
    Serial.print("ChkMatch= ");
    Serial.println(ChkMatch);
    Serial.print("ValidateChecksum = ");
    Serial.println(ValidateChecksum);
    Serial.print("inputgroup[]=");
    Serial.println(*inputgroup);
    Serial.print("byteMode =");
    Serial.println(byteMode);
    Serial.print("Reset = ");
    Serial.println(Reset);
    Serial.print("XOR= ");
    Serial.println(XOR);
    Serial.print("buffer[] = ");
    Serial.println(buffer);
    Serial.println(" Variables Print");
    Serial.println("==============================================================================");

    PrintVariables = 0; 
  }
  return;
} //PrintVariables
void ResetVariables(){ //reset variables
  PrintVariablesList ();
  // variables
  cnt=0;                    // Reset the buffer
  for (int i=0;i<300;i++){    // 
    inputData[i]=' ';
  }
  group = 0;               // Reset the group bit for next time sentence is received
  group1 =0;
  stat =0;               // Reset the status bit for next time sentence is received
  check=0;
  //char hx[5] = "0x00";
  //hx[5] = 0x00;
  hx[0] = '\0';
  hx[1] = 'x';
  hx[2] = '\0';
  hx[3] = '\0';
  hx[4] = '\0';

  char buffer[5] = "0x00";
  nmeaChk = 0;
  calcChecksum=0;
  stopXOR = 0;
  byteCount=0;
  ChkMatch= 0;
  ValidateChecksum = 0;
  inputgroup[10]=0;
  cnt=0;
  //byteMode =0;
  Reset = 0;
  XOR= 0;
  ProceedAllowed =0;
  tstMode=0;
  Nogood=0;
  li1= 0;
  li2= 0;
  char MrlID[4] = "$MR";
  byteCount=0;
  ReturnMode=0;
  Serial.println("Variables are Reset");
  //softReset();
  return;
}  //reset variables

void setup() {
 Serial.begin(57600);        // connect to the serial port

 softReset();

 // --VENDOR SETUP BEGIN--
 // --VENDOR SETUP END--
}

//
void softReset()
{

 for (int i = 0; i < MAX_SERVOS - 1; ++i)
 {
  servoSpeed[i] = 100;
  servos[i].detach();
 }

 for (int j = 0; j < DIGITAL_PIN_COUNT - 1; ++j)
 {
  pinMode(j, OUTPUT);
 }

 digitalReadPollingPinCount = 0;
 analogReadPollingPinCount = 0;
 loopCount = 0;

}

void setPWMFrequency (int address, int prescalar)
{
 int clearBits = 0x07;
 if (address == 0x25)
 {
  TCCR0B &= ~clearBits;
  TCCR0B |= prescalar;
 } else if (address == 0x2E)
 {
  TCCR1B &= ~clearBits;
  TCCR1B |= prescalar;
 } else if (address == 0xA1)
 {
  TCCR2B &= ~clearBits;
  TCCR2B |= prescalar;
 }

}

void removeAndShift (int array [], int& len, int removeValue)
{
 int pos = -1;

 if (len == 0)
 {
  return;
 }

 // find position of value
 for (int i = 0; i < len; ++i)
 {
  if (removeValue == array[i])
  {
   pos = i;
   break;
  }
 }
 // if at the end just decrement size
 if (pos == len - 1)
 {
  --len;
  return;
 }

 // if found somewhere else shift left
 if (pos < len && pos > -1)
 {
  for (int j = pos; j < len - 1; ++j)
  {
   array[j] = array[j+1];
  }
  --len;
 }
}

/*
* getCommand - retrieves a command message
 * input messages are in the following format
 *
 * command message - (4 byte protocol) MAGICNUMBER|FUNCTION|DATA0|DATA1
 * e.g. digitalWrite (13, 1)     = DIGITAL_WRITE|13|1 = 170|0|13|1
 *
 * return message  - (5 byte protocol) MAGICNUMBER|FUNCTION|DATA0|MSB|LSB
 * e.g. results of analogRead = ANALOG_READ|3|1|1  = 170|3|3|257
 *
 * $MRCMD,xxx,xxx*FF
 * $MRCMD,000,013,001*4A
 * checksum ref http://www.hhhh.org/wiml/proj/nmeaxor.html
 *
 */
           /*
$MRCMD,000,013,001*4A
$MRCMD,000,013,000*4B
$MRTST,000,013,000,1,2,3,4
$MRTST,6,11,0,
$MRTST,7,11,50,100,
$MRTST,0,12,0,
$MRTST,0,12,1,
 */

boolean getCommand ()
{
  // handle serial data begin
  if (Serial.available() > 0)
  {
    // read the incoming byte:
    newByte = Serial.read();

    if (byteCount == 0 && newByte != MAGIC_NUMBER)
    {
      // ERROR !!!!!
      // TODO - call modulus error method - notify sender

      //++errorCount;
      //return false;
      byteMode=1;
      //++byteCount;
    }
    if (byteCount == 0 && newByte == MAGIC_NUMBER)
    {
      byteMode=0; //Ensure MRL Natural protocol
    }
    if (byteMode==0){ //MRL Natural
      ioCommand[byteCount] = newByte;
      ++byteCount;

      if (byteCount > 3)
      {
        return true;
      }
      else{
        return false;
      }

    } //MRL Natural
    if (byteMode==1){ //MRL Direct Serial
      //

      inputData[cnt] = newByte;        // If there is serial port data, it is put in the buffer
      cnt++;
      if (newByte==13)            // If the received byte is = to 13, end of transmission
      {

        //PrintVariables = 0;
        //PrintVariablesList();
        Serial.println("----------------------");
        Serial.println(inputData); 
        Serial.println("----------------------");
        //for (int ij=0;ij<3;ij++){     // Verifies if the received command starts with $MR
        if (inputData[0] == MrlID[0]&& inputData[1] == MrlID[1]&&inputData[2] == MrlID[2]){
          stat=3;
          Serial.println("---Good------");
          // Serial.println(stat);
        }
        else{
          Nogood = 1;
          Serial.println("---No Good-----");
          //Serial.println(stat);
        }
        //}
      }
      //Serial.println("stat=");Serial.println(stat);
      //if (Nogood = 1 && stat <3){//$MR not found
      //  ReturnMode = 2;
      //  Reset =1;
      //}
      if(newByte==13 && stat==3){               // If $MR, continue and process the data
        //Serial.println("status stat==3");
        for (int i=0;i<cnt;i++){
          if (i==5){
            //Serial.print("status i=="); Serial.println(i);
            //Serial.print("cnt="); Serial.println(cnt);
            // since $MR assume valid command incoming
            Cmd[0] = inputData[i-2]; 
            Cmd[1] = inputData[i-1]; 
            Cmd[2] = inputData[i];  
            //Serial.print("Cmd:"); Serial.println(Cmd);
            // sets Cmd = to buffer data position 3-5
            if (Cmd[0]=='t'|Cmd[0]=='T'&&Cmd[1]=='s'|Cmd[1]=='S'&&Cmd[2]=='t'|Cmd[2]=='T'){
              ValidateChecksum = 1;
              stopXOR = 1;
              ProceedAllowed =1;
              tstMode=1;
              Serial.println("No Checksum Required for Test cmd");
            }//tst mode, no checksum required
            else{
              ValidateChecksum = 0; // checksum required
              stopXOR = 0;
              ProceedAllowed =0;
              Serial.println("Checksum Required for CMD");
            }

          }
          if (inputData[i]==','){    // check for the "," separator and set a group relationship to it.
            //Serial.println("inputData[i]==','");
            subSection[group]=i;
            group++;
          }

          if (inputData[i]=='*'){    // check for the "*"
            //Serial.println("inputData[i]=='*'");
            stopXOR = 1;
            subSection[group]=i;   // no more commas so set last group bounds
            inputgroup[group] += inputData[i];
            group++;
            Checksum[0] = inputData[i+1];
            Checksum[1] = inputData[i+2];
            Checksum[2] = 0x00;
            //Serial.println("Found *");
            //String BufferChecksum = char(Checksum[1]) & char(Checksum[2])

            //calcChecksum ^=  check;
            //-----------------

            //-----------------
            //Serial.print("BufferChecksum=");
            //Serial.print(BufferChecksum);
            // Serial.print("Calculated checksum=:");
            // Serial.println(calcChecksum);
          }
          if (inputData[i] !='*'){
            //Serial.println("inputData[i] !='*'");
            // still getting data
            //-----------------
            if (( inputData[i] != '$') && stopXOR == 0){
              check ^= (inputData[i]);
              //Serial.print("check=");
              //Serial.print(inputData[i]);
              //Serial.print("<-in : check = ");
              //Serial.println(check);

              //-----------------
              //Reset=1;
              //byteCount=0;
            }
          } //!='*'
          //Serial.println(inputData[i]);
        }  // i==5 cmd group

        if (ValidateChecksum == 0 && stopXOR == 1){
          //Checksum compare the check (decimal) to Checksum[1] and Checksum[2]
          // below structure of serial port received checksome data

          //hx[1] = 0x00;
          hx[2] = Checksum[0];
          hx[3] = Checksum[1];
          hx[1] = 'x';
          hx[0] = '0';
          nmeaChk = atoi(hx); //hx[2] + hx[3];
          //below structure of calculated checksum xor data
          sprintf(buffer,"0x%2X",check);
          //Serial.print(buffer);Serial.println(hx);
          //Serial.print(buffer);
          //Serial.print("<- Hex calc. checksum : Serial checksum ->");
          //Serial.println(hx);
          PrintVariables = 1;
          PrintVariablesList();
          if (strcmp(buffer,hx)) { // false match
            Serial.println("No Checksum Match");
            ValidateChecksum = 1;
            ChkMatch= 0; // 1=match 0 = no match/abort
            ProceedAllowed =0;
            ReturnMode = 2;
            ResetVariables();
          }
          else{  // true match
            Serial.println("Checksum's Match");
            //Serial.print(buffer);
            //Serial.print("<- Hex calc. checksum: Serial checksum ->");
            //Serial.println(hx);
            ValidateChecksum = 1;
            ChkMatch= 1; // 1=match 0 = no match/abort
            ProceedAllowed =1;
          }
        }//end checksum check required
        //Serial.print("group=");Serial.println(group);
        if (ValidateChecksum == 1 && ProceedAllowed ==1){ //display output
          for (int ii=0;ii<10;ii++){    //  Clear ioCommand[]
            ioCommand[ii]=0;
            iochar[ii]=0;
          }
          //Serial.println("--ioCommand--");
          //Serial.println(ioCommand[0]);
          //Serial.println(ioCommand[1]);
          //Serial.println(ioCommand[2]);
          //Serial.println(ioCommand[3]);

          for (int i=0;i<group+1;i++){  //good data return
            switch(i){
            case 0 :
              Serial.print("Group 1: ");
              break;
            case 1 :
              Serial.print("Group 2: ");
              break;
            case 2 :
              Serial.print("Group 3: ");
              break;
            case 3 :
              Serial.print("Group 4: ");
              break;
            case 4 :
              Serial.print("Group 5: ");
              break;
            case 5 :
              Serial.print("Group 6: ");
              break;
            case 6 :
              Serial.print("Group 7: ");
              break;
            case 7 :
              Serial.print("Group 8: ");
              break;
            case 8 :
              Serial.print("Group 9: ");
              break;
            case 9 :
              Serial.print("Group 10: ");
              break;
            case 10 :
              Serial.print("Group 11: ");
              break;
            case 11 :
              Serial.print("Group 12: ");
              break;
            }
            int q =0;
            //*iochar=0x48;
            li1=0;
            li2=0;
            //*pEnd=-1;
            for (int m=subSection[i];m<(subSection[i+1]-1);m++){
              //if (inputData[m+1] == '/0'){inputData[m+1] =' ';}             
              iochar[q]=inputData[m+1];
              //Serial.print("m=");Serial.println(m);
              Serial.print(inputData[m+1]);

              q++;
              //-----------------------------------
              // How to format what I have to be useable in MRL format?....

            }
            iochar[q]=' ';
            Serial.println("");
            //-------Set ioCommand[]--------------

            //long int li1;
            li1='      ';
            li1 =strtol (iochar,&pEnd,10);
            char buffera [10]="         ";
            char buffers [10]="         ";
            sprintf (buffera, "%X", li1);
            sprintf (buffers, "%s", buffera);
            unsigned char li3 =strtol (buffera,&pEnd,16);
            if (q==1&&*iochar=='0'){
              li3=0;
              }
            if (q==0){
              li3=-1;
              }
            ioCommand[i+1] =li3;
            //Serial.print("li1=");Serial.println(li1);
            //Serial.print("li3=");Serial.println(li3);
          }//good data return

          ReturnMode = 1;
          ioCommand[0] = MAGIC_NUMBER;
        } //display output
      }  //stat==3 

      if (Nogood == 1 && stat !=3){
        Serial.println("Nogood ==1 and stat!=3");
        ReturnMode = 2;
        Reset =1;
      }

      if (tstMode=1 && byteCount > 300){
        ReturnMode = 0;
        ResetVariables();
      }
      if (byteMode ==1  && stat>=25){
        Serial.println("byteMode ==0 && newByte ==13 && stat>=5");
        ++errorCount;
        byteMode =0;
        ReturnMode = 2;
        Reset =1;
      }
      if (ReturnMode == 1){
        // ReturnMode =0;
        Serial.println("");
        PrintVariables = 0;
        // Serial.println("---------------");

        //for (int n = 0 ;n<10;n++){
        //  Serial.println(inputgroup[n]);
        //}

        Serial.println("--ioCommand--");
        Serial.println(ioCommand[0]);
        Serial.println(ioCommand[1]);
        Serial.println(ioCommand[2]);
        Serial.println(ioCommand[3]);
        Serial.println(ioCommand[4]);
        Serial.println(ioCommand[5]);
        Serial.println(ioCommand[6]);
        ResetVariables();
        Serial.println("----------------------");
        return true;
      }
      if (ReturnMode == 2){
        ReturnMode =0;
        ResetVariables();
        Serial.println("Return false requested!");

        return false;
      }

    }//byteMode ==1

  } // if Serial.available

    //return false;
}

/*
set pollingRead = true if any of the pins are commanded to read
default behavior should be to poll read
*/

void loop () {

 ++loopCount;

 if (getCommand())
 {
  /*
  must comment out debugging - if serial control is used
  all Serial, debugging assumes no
  interference will occur due to serial
  processing
  */

  /*
  Serial.print("c[");
  Serial.print(ioCommand[0],HEX);
  Serial.print("|");
  Serial.print(ioCommand[1],HEX);
  Serial.print("|");
  Serial.print(ioCommand[2],HEX);
  Serial.print("]\n");
  */

  switch (ioCommand[1])
  {
  case DIGITAL_WRITE:
   digitalWrite(ioCommand[2], ioCommand[3]);
   break;
  case ANALOG_WRITE:
   analogWrite(ioCommand[2], ioCommand[3]);
   break;
  case PINMODE:
   pinMode(ioCommand[2], ioCommand[3]);
   break;
  case PULSE_IN:
   retULValue = pulseIn(ioCommand[2], ioCommand[3]);
   break;
  case SERVO_ATTACH:
   servos[ioCommand[2]].attach(ioCommand[3]);
                        break;                               
  case SERVO_WRITE:

   if (servoSpeed[ioCommand[2]] == 100) // move at regular/full 100% speed
   {  
    // move at regular/full 100% speed
    // although not completely accurate
    // target position & current position are
    // updated immediately
     servos[ioCommand[2]].write(ioCommand[3]);
                                servoTargetPosition[ioCommand[2]] = ioCommand[3];
    servoCurrentPosition[ioCommand[2]] = ioCommand[3];
   } else if (servoSpeed[ioCommand[2]] < 100 && servoSpeed[ioCommand[2]] > 0) {
    // start moving a servo at fractional speed
    servoTargetPosition[ioCommand[2]] = ioCommand[3];
    movingServos[movingServosCount]=ioCommand[2];
    ++movingServosCount;
   } else {
    // NOP - 0 speed - don't move
   }
   break;
  case SET_SERVO_SPEED:
   // setting the speed of a servo
   servoSpeed[ioCommand[2]]=ioCommand[3];
   break;
  case SERVO_SET_MAX_PULSE:
   //servos[ioCommand[2]].setMaximumPulse(ioCommand[3]);    TODO - lame fix hardware
   break;
  case SERVO_DETACH:
   servos[ioCommand[2]].detach();
   break;
  case SET_PWM_FREQUENCY:
   setPWMFrequency (ioCommand[2], ioCommand[3]);
   break;
  case ANALOG_READ_POLLING_START:
   analogReadPin[analogReadPollingPinCount] = ioCommand[2]; // put on polling read list
   analogPinService[ioCommand[2]] |= POLLING_MASK;
   // TODO - if POLLING ALREADY DON'T RE-ADD - MAKE RE-ENTRANT - if already set don't increment
   ++analogReadPollingPinCount;
   break;
  case ANALOG_READ_POLLING_STOP:
   // TODO - MAKE RE-ENRANT
   removeAndShift(analogReadPin, analogReadPollingPinCount, ioCommand[2]);
   analogPinService[ioCommand[2]] &= ~POLLING_MASK;
   break;
  case DIGITAL_READ_POLLING_START:
   // TODO - MAKE RE-ENRANT
   digitalReadPin[digitalReadPollingPinCount] = ioCommand[2]; // put on polling read list
   ++digitalReadPollingPinCount;
   break;
  case DIGITAL_READ_POLLING_STOP:
   // TODO - MAKE RE-ENRANT
   removeAndShift(digitalReadPin, digitalReadPollingPinCount, ioCommand[2]);
   digitalPinService[ioCommand[2]] &= ~POLLING_MASK;
   break;
  case SET_ANALOG_TRIGGER:
   // TODO - if POLLING ALREADY DON'T RE-ADD - MAKE RE-ENTRANT
   analogReadPin[analogReadPollingPinCount] = ioCommand[2]; // put on polling read list
   analogPinService[ioCommand[2]] |= TRIGGER_MASK;
   ++analogReadPollingPinCount;
   break;
  case DIGITAL_DEBOUNCE_ON:
   debounceDelay = 50;
   break;
  case DIGITAL_DEBOUNCE_OFF:
   debounceDelay = 0;
   break;
  case DIGITAL_TRIGGER_ONLY_ON:
   digitalTriggerOnly = true;
   break;
  case DIGITAL_TRIGGER_ONLY_OFF:
   digitalTriggerOnly = false;
   break;
  case SET_SERIAL_RATE:
   Serial.end();
   delay(500);
   Serial.begin(ioCommand[2]);
   break;
   
  case SOFT_RESET:
   softReset();
   break;

   // --VENDOR CODE BEGIN--
   // --VENDOR CODE END--

  case NOP:
   // No Operation
   break;
  default:
   //             Serial.print("unknown command!\n");
   break;
  }

  // reset buffer
  ioCommand[0] = -1; // MAGIC_NUMBER
  ioCommand[1] = -1; // FUNCTION
  ioCommand[2] = -1; // PARAM 1
  ioCommand[3] = -1; // PARAM 2
  byteCount = 0;

 } // if getCommand()

 // digital polling read - send data for pins which are currently in INPUT mode only AND whose state has changed
 for (int i  = 0; i < digitalReadPollingPinCount; ++i)
 {
  if (debounceDelay)
  {
    if (millis() - lastDebounceTime[digitalReadPin[i]] < debounceDelay)
    {
      continue;
    }
  }
 
  // read the pin
  readValue = digitalRead(digitalReadPin[i]);

  // if my value is different from last time  && config - send it
  if (lastDigitalInputValue[digitalReadPin[i]] != readValue  || !digitalTriggerOnly)
      if (lastDigitalInputValue[digitalReadPin[i]] != readValue  || !digitalTriggerOnly)
    {if (byteMode==0){ //MRL protocol
      Serial.write(MAGIC_NUMBER);
      Serial.write(DIGITAL_VALUE);
      Serial.write(digitalReadPin[i]);// Pin#
      Serial.write(readValue >> 8);   // MSB
      Serial.write(readValue);  // LSB
      }else{ //DirectSerial
      Serial.print(MAGIC_NUMBER);
      Serial.print("|");
      Serial.print(DIGITAL_VALUE);
      Serial.print("|");
      Serial.print(digitalReadPin[i]);// Pin#
      Serial.print("|");
      Serial.print(readValue >> 8);   // MSB
      Serial.print("_");
      Serial.println(readValue);  // LSB
      }
      lastDebounceTime[digitalReadPin[i]] = millis();
     
    }

  // set the last input value of this pin
  lastDigitalInputValue[digitalReadPin[i]] = readValue;
 }

 // analog polling read - send data for pins which are currently in INPUT mode only AND whose state has changed
 for (int i  = 0; i < analogReadPollingPinCount; ++i)
 {
  // read the pin
  readValue = analogRead(analogReadPin[i]);

  // if my value is different from last time - send it
      if (lastAnalogInputValue[analogReadPin[i]] != readValue   || !analogTriggerOnly) //TODO - SEND_DELTA_MIN_DIFF
    {if (byteMode==0){ //MRL protocol
      Serial.write(MAGIC_NUMBER);
      Serial.write(ANALOG_VALUE);
      Serial.write(analogReadPin[i]);
      Serial.write(readValue >> 8);   // MSB
      Serial.write(readValue & 0xFF); // LSB
      }else{ //DirectSerial
      Serial.print(MAGIC_NUMBER);
      Serial.print("|");
      Serial.print(ANALOG_VALUE);
      Serial.print("|");
      Serial.print(analogReadPin[i]);
      Serial.print("|");
      Serial.print(readValue >> 8);   // MSB
      Serial.print("_");
      Serial.println(readValue & 0xFF); // LSB
      }
    }
  // set the last input value of this pin
  lastAnalogInputValue[analogReadPin[i]] = readValue;
 }

 // handle the servos going at fractional speed
 for (int i = 0; i < movingServosCount; ++i)
 {
  int servoIndex = movingServos[i];
  int speed = servoSpeed[servoIndex];
  if (servoCurrentPosition[servoIndex] != servoTargetPosition[servoIndex])
  {
   // caclulate the appropriate modulus to drive
   // the servo to the next position
   // TODO - check for speed > 0 && speed < 100 - send ERROR back?
   int speedModulus = (100 - speed) * 10;
   if (loopCount % speedModulus == 0)
   {
    int increment = (servoCurrentPosition[servoIndex]<servoTargetPosition[servoIndex])?1:-1;
    // move the servo an increment
    servos[servoIndex].write(servoCurrentPosition[servoIndex] + increment);
    servoCurrentPosition[servoIndex] = servoCurrentPosition[servoIndex] + increment;
   }
  } else {
   removeAndShift(movingServos, movingServosCount, servoIndex);
  }
 }

} // loop