#include <due_can.h>

/*
 * Data structure to hold the commands to send to the CANBUS
 */
struct Command{
  String theCommand;
  String description;
  CAN_FRAME canFrame;
  uint8_t timesToSend;//how many times to send the command
  uint16_t delayBetweenSends; //delay in miliseconds between sends
  String commandAfter;//if we want to send another command right away, list it here

  //Constructor
  Command(String cCMD, String cDescription, uint32_t cID, uint8_t cExtended, uint8_t cLength,
          uint8_t cD0, uint8_t cD1, uint8_t cD2, uint8_t cD3, uint8_t cD4, uint8_t cD5, uint8_t cD6, uint8_t cD7,
          uint8_t cTTS, uint16_t cDelay,String cCommandAfter){
    theCommand = cCMD;
    description = cDescription;
    canFrame.id = cID;
    canFrame.extended = cExtended;
    canFrame.length = cLength;
    canFrame.data.byte[0] = cD0;
    canFrame.data.byte[1] = cD1;
    canFrame.data.byte[2] = cD2;
    canFrame.data.byte[3] = cD3;
    canFrame.data.byte[4] = cD4;
    canFrame.data.byte[5] = cD5;
    canFrame.data.byte[6] = cD6;
    canFrame.data.byte[7] = cD7;
    timesToSend = cTTS;
    delayBetweenSends = cDelay;
    commandAfter = cCommandAfter;
  }
};

/*
 * Bunch of data structures to hold the various frames sent to/from the xbee cell modem
 * This isn't all of them, just the ones I'm using.
 */

typedef struct{
/*
 * TX SMS Frame      BYTE
 * Start Delimiter | 1   | 0x7E
 * Length          | 2-3 |
 * Frame Type      | 4   | 0x1F for TX SMS
 * Frame ID        | 5   | Reference identifier used to match status responses. 0 disables the TX Status frame.
 * Options         | 6   | "Reserved for future use."
 * Phone Number    | 7-27| 20 bytes
 * Message         | 28-n| 160 max for SMS
 * Checksum        | n-1 |
 */
  const uint8_t startDelimiter = 0x7E;
  uint8_t frameLengthLow = 0;
  uint8_t frameLengthHigh= 0;
  const uint8_t frameType = 0x1F;
  uint8_t frameID = 0;//Reference identifier used to match status responses. 0 disables the TX Status frame.
  const uint8_t options = 0;//"reserved for future use."
  char phoneNumber[20] = {};
  char payload[160] = {};
  uint8_t checksum = 0;
} frameTX_SMS;

//Transmit (TX) Status - 0x89
//Indicates the success or failure of a transmit operation.
typedef struct{
  const uint8_t frameType = 0x89;
  uint8_t frameID = 0;//Refers to the frame ID specified in a previous transmit frame
  uint8_t status;
/*
 * Code  Description
 * 0x00   Successful transmit
 * 0x21  Failure to transmit to cell network
 * 0x22  Not registered to cell network
 * 0x2c  Invalid frame values (check the phone number)
 * 0x31  Internal error
 * 0x32  Resource error (retry operation later)
 * 0x74  Message too long
 * 0x78  Invalid UDP port
 * 0x79  Invalid TCP port
 * 0x7A  Invalid host address
 * 0x7B  Invalid data mode
 * 0x80  Connection refused
 * 0x81  Socket connection lost
 * 0x82  No server
 * 0x83  Socket closed
 * 0x84  Unknown server
 * 0x85  Unknown error
 */
} frameTX_SMS_Status;

typedef struct{
  const uint8_t frameType = 0x9F;
  uint8_t phoneNumber[20] = {}
  uint8_t payload[160] = {};//Body of the received SMS message.
} frameRX_SMS;

typedef struct{
  const uint8_t startDelimiter = 0x7E;
  uint8_t frameLengthLow = 0;
  uint8_t frameLengthHigh= 0;
  uint8_t frameType = 0x08;
  uint8_t frameID = 0x01;//Identifies the data frame for the host to correlate with a subsequent ACK. If set to 0, the device does not send a response.
  uint8_t ATcommand[2];
  uint8_t checksum = 0x00;
} frameTX_AT_Command;

typedef struct{
  const uint8_t frameType = 0x88;
  uint8_t frameID = 0;//Identifies the data frame for the host to correlate with a subsequent ACK. If set to 0, the device does not send a response.
  char ATcommand[2];//Command name: two ASCII characters that identify the AT command.
  uint8_t status; //0x00 = OK, 0x01 = ERROR, 0x02=Invalid Command, 0x03=Invalid Parameter, 0x04=Tx Failure.
  uint8_t parameterValue[256] = {};//Register data in binary format. If the register was set, then this field is not returned.
} frameRX_AT_Command;






//if you want to use the digi XCTU software, uncomment this
//#define XCTU

#define DELAY 2 //delay when reading from the serial

#define redLED DS2
#define grnLED DS6

//This made it easier to tell which serial I was writing to.
#define digiSerial Serial

//SMS commands to the CANBUS will be this many characters only.
#define CMDLENGTH 5

//cell number for M2 to send SMS
//"Use numbers and the + symbol only, no other symbols or letters."
const String MYCELL = "+14508088900";

/*
 * An array of the Command struct I created. Contains the CAN_FRAME
 * struct as well as a bunch of other info.
 * These are all the commands that it would be possible to excute via SMS.
 */
const int numCommands = 4;
Command cbCommands[numCommands] = {
  Command("START", "Start Truck",  0x111,false,6,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,4,1500,"RAOFF"),
  Command("RAOFF", "Radio Off",    0x222,false,8,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,1,10,""),
  Command("HFULL", "Heat on full", 0x333,false,5,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,1,1,""),
  Command("CLOFF", "Climate Off",  0x444,false,5,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,1,1,"")
                                  };

void setup() {
  //Initialize and turn on the LEDs until setup complete
  for(int x=14;x<=18;x++){
    pinMode(x,OUTPUT);
    digitalWrite(x,LOW);
  }

  digiSerial.begin(9600);          // Serial port for XBEE socket
  SerialUSB.begin(9600);           // Native USB port

  pinMode(XBEE_MULT2, INPUT);      // DIGI Associate PIN
  pinMode(XBEE_RST, OUTPUT);
  digitalWrite(XBEE_RST, HIGH);    // Make sure RESET is HIGH

  Can0.begin(CAN_BPS_500K);

  //Setup complete, turn off the LEDs
  for(int x=14;x<=18;x++){
    digitalWrite(x,HIGH);
  }
  //leave the red LED on so I know there is power.
  digitalWrite(DS2,LOW);
}

/**
 * Sends a CANBUS message.
 *
 * @param command The command to send of type Command
 * @return nothing
 */
void txCANBUS(Command cmd){
  char str[1];
  String message = "";
  for(unsigned int x=0;x<sizeof(cmd);x++){
    sprintf(str,"%.2X",*((char *)(&cmd)+x));
    message = message + str + " ";
  }
  SerialUSB.println(message);
  for(int x=0;x<cmd.timesToSend;x++){
    SerialUSB.print("Sending: ");SerialUSB.println(cmd.description);
/***********
 *This line sends the actual CANBUS message. Comment out as required.
 ************/
//    Can0.sendFrame(cmd.canFrame);
    SerialUSB.print("Waiting (ms): ");SerialUSB.println(cmd.delayBetweenSends);
    delay(cmd.delayBetweenSends);
  }
  if(cmd.commandAfter != ""){
    //If the current command has a command to run right after
    SerialUSB.print("Running right after: ");SerialUSB.println(cmd.commandAfter);
    delay(1000);
    txCANBUS(cbCommands[findCommandIndex(cmd.commandAfter)]);
  }
}

/**
 * Finds the index of the given command in the global array of commands
 *
 * @param command The command name to find to find as type String.
 * @return The index number when found. -1 for not found.
 */
int findCommandIndex(String str){
  int index = -1;
  for(int x=0;x<numCommands;x++){
    if(cbCommands[x].theCommand == str){
      index = x;
      break;
    }
  }
  return index;
}

/*
 * Calculate the checksum for the digi xbee modem frames
 *
 * To calculate checksum of digi xbee frame:
 * Add all bytes of the packet, except the start delimiter 0x7E and the length (the second and third bytes).
 * Keep only the lowest 8 bits from the result.
 * Subtract this quantity from 0xFF.
 *
 * @param frame A character array containing all the frame data
 * @param length the length of the frame
 */
uint8_t calculateChecksum(char * frame, unsigned int l){
  unsigned int total = 0;
  //since I'm sending the whole frame, skip the first three bytes
  for(unsigned int x=3;x<l+3;x++){
    total += frame[x];
  }
  unsigned char low8bits = total & 0xFF;
  char checksum = 0xFF - low8bits;
  return checksum;
}

/**
 * Sends an SMS text message.
 *
 * @param message The text message to send
 * @param messageLength The length of the message
 * @param phoneNumber The phone number to send to
 * @return nothing
 */
void txSMS(char * msg,int msgLength,String phoneNumber){
  if(msgLength == 0){//no message
    return;
  }
  frameTX_SMS frame;
  //need to make sure phoneNumber isn't > 20
  strcpy(frame.phoneNumber, phoneNumber.c_str());
  //should verify this size too...
  strcpy(frame.payload, msg);
  frame.frameLengthHigh = 20 + msgLength + 1 + 1 + 1;//1 FrameType, 1 FrameID, 1 Options.
  frame.checksum = calculateChecksum((char *)(&frame),frame.frameLengthHigh);
  //Since SMS can be 160 characters and I read dynamically allocating memory in arduino should be avoided,
  //the struct has an array of 160 chars initialized to 0x00. Put the checksum as the last "character" of that
  //string so as not to TX 160 characters, mostly nulls, everytime.
  frame.payload[strlen(frame.payload)] = frame.checksum;

  int frameTotalLength = 3 + frame.frameLengthHigh + 1;
/***********
 *This line sends the actual SMS message. Comment out as required.
 ************/
//  digiSerial.write((char*)(&frame), frameTotalLength); delay(100);

  //print out what's going on to the console
  char str[1];
  String message = "";
  for(int x=0;x<frameTotalLength;x++){
    sprintf(str,"%.2X",*((char *)(&frame)+x));
    message = message + str + " ";
  }
  SerialUSB.print("\nTX SMS (HEX): ");
  SerialUSB.println(message);
  SerialUSB.println((char*)frame.phoneNumber);
  frame.payload[strlen(frame.payload)-1] = 0x00;//remove the checksum before printing
  SerialUSB.println((char*)frame.payload);
}

void loop() {
  while (SerialUSB.available()) {     // read from port 1, send to port 0:
    int inByte = SerialUSB.read();
    digiSerial.write(inByte);
    delay(DELAY);
  }

  if(digiSerial.available()){
#ifdef XCTU
    while (digiSerial.available()) {        // read from port 0, send to port 1:
      char inByte = digiSerial.read();
      SerialUSB.write(inByte);
      delay(DELAY);
    }
#else
    //Read the first byte. If it's not 0x7E, skip.
    //All frames start with 0x7E
    if(digiSerial.read() == 0x7E){
      delay(DELAY);
      //read the next 3 bytes of the header. 2 bytes for size and 1 for frame type
      uint16_t lengthOfFrame;
      lengthOfFrame = digiSerial.read(); delay(DELAY); //Size 1/2
      lengthOfFrame <<= 8;
      lengthOfFrame |= digiSerial.read(); delay(DELAY); //Size 2/2
      byte frameType = digiSerial.read(); delay(DELAY); //Frame Type

      //What frame type are we dealing with?
      if(frameType == 0x8A){//Modem Status
        byte status   = digiSerial.read(); delay(DELAY); //status
        digiSerial.read(); delay(DELAY); //read and discard the checksum.
        String message = "Modem Status: ";
        char str[2];
        switch(status){
          case 0x00: message += "0x00 - Hardware reset or power up";         break;
          case 0x01: message += "0x01 - Watchdog timer reset";               break;
          case 0x02: message += "0x02 - Registered with cellular network";   break;
          case 0x03: message += "0x03 - Unregistered with cellular network"; break;
          case 0x0E: message += "0x0E - Remote Manager connected";           break;
          case 0x0F: message += "0x0F - Remote Manager disconnected";        break;
          default:   message += "Unknown Modem Status Value: ";
                     sprintf(str,"0x%.2X",status);
                     message+= str;
        }
        SerialUSB.println(message);
      }else if(frameType == 0x89){//TX Status
        //not sure what to do with this. Perhaps save to SD card log?
        //When the frameID field of the TX_SMS frame is set to 0 (default), this status message
        //will not even be generated.
        frameTX_SMS_Status frame;
        frame.frameID = digiSerial.read(); delay(DELAY);
        frame.status  = digiSerial.read(); delay(DELAY);
        digiSerial.read(); delay(DELAY); //read and discard the checksum.
      }else if(frameType == 0x88){ //AT Command Response - 0x88
        frameRX_AT_Command frame;
        frame.frameID = digiSerial.read(); delay(DELAY);
        frame.ATcommand[0] = digiSerial.read(); delay(DELAY);
        frame.ATcommand[1] = digiSerial.read(); delay(DELAY);
        frame.status = digiSerial.read(); delay(DELAY);

        //next read in the data
        int payloadLength = lengthOfFrame - 1 - 1 - 2 - 1;//1 FrameType, 1 FrameID, 2 AT command, 1 status
        for(int x=0;x<payloadLength;x++){
          frame.parameterValue[x] = digiSerial.read(); delay(DELAY);
        }
        byte checksum = digiSerial.read(); delay(DELAY);

        //Print the frame out to the console.
        char str[1];
        String message = "7E ";
        sprintf(str,"%.2X",lengthOfFrame & 0xFF00);
        message = message + str + " ";
        sprintf(str,"%.2X",lengthOfFrame & 0x00FF);
        message = message + str + " ";
        for(int x=0;x<lengthOfFrame;x++){
          sprintf(str,"%.2X",*((char *)(&frame)+x));
          message = message + str + " ";
        }
        sprintf(str,"%.2X",checksum);
        message += str;
        SerialUSB.println("\nAT Command Response");
        SerialUSB.println(message);
        SerialUSB.print("AT ");
        SerialUSB.write(frame.ATcommand[0]);
        SerialUSB.write(frame.ATcommand[1]);
        SerialUSB.print(" response from modem: ");
        SerialUSB.println((char*)frame.parameterValue);

        //Send the reply out as SMS
        SerialUSB.println("\nForwarding AT reply out by SMS");
        txSMS((char*)frame.parameterValue,strlen((char*)frame.parameterValue),MYCELL);
      }else if(frameType == 0x9F){//RX SMS
        frameRX_SMS frame;
        //read in the phone number. 20 bytes.
        for(int x=0;x<20;x++){
          frame.phoneNumber[x] = digiSerial.read(); delay(DELAY);
        }
        //next read in the data
        int payloadLength = lengthOfFrame - 1 - 20;//1 for frame type, 20 for PH.
        for(int x=0;x<payloadLength;x++){
          frame.payload[x] = digiSerial.read(); delay(DELAY);
        }
        //lastly read in the checksum. I'm going to wrongly assume all the frames are good to go
        //because I can't be bothered to write a verrify checksum function at the moment. But I
        //think the cellular modem should reject it if the checksum is invalid.
        byte checksum = digiSerial.read(); delay(DELAY);

        char str[1];
        String message = "7E ";
        sprintf(str,"%.2X",lengthOfFrame & 0xFF00);
        message = message + str + " ";
        sprintf(str,"%.2X",lengthOfFrame & 0x00FF);
        message = message + str + " ";
        for(int x=0;x<lengthOfFrame;x++){
          sprintf(str,"%.2X",*((char *)(&frame)+x));
          message = message + str + " ";
        }
        sprintf(str,"%.2X",checksum);
        message += str;
        SerialUSB.print("\nRX SMS: "); SerialUSB.println(message);
        SerialUSB.print("From: ");     SerialUSB.println((char*)frame.phoneNumber);
        SerialUSB.print("Message: ");  SerialUSB.println((char*)frame.payload);

        if(frame.payload[0] == 'D' && frame.payload[1] == 'S'){//toggle an LED
          //no real practical use for this I suppose. Just helpful for testing.
          //Send the pin name (ex DS2), take the 3rd character(ascii value), subtract 0x30, and add 12 to get the M2 board pindescription value.
          byte led = frame.payload[2] - 0x30 + 12;
          if(led >= 14 && led <= 18){
            digitalWrite(led,((digitalRead(led) == HIGH) ? LOW : HIGH));
          }
        }else if(frame.payload[0] == 'A' && frame.payload[1] == 'T'){//send an AT command
          frameTX_AT_Command ATframe;
          unsigned short len = 4;//AT frames are 4 bytes
          ATframe.frameLengthHigh = len & 0xFF;
          len <<= 8;
          ATframe.frameLengthLow =len & 0xFF;
          ATframe.ATcommand[0] = frame.payload[2];
          ATframe.ATcommand[1] = frame.payload[3];
          ATframe.checksum = calculateChecksum((char*)(&ATframe),4);

          //length is 1 start delimiter, 2 size,
          //1 frame type, 1 frame id, 2 AT command, 1 cksum.
          //1 + 2 + 1 + 1 + 2 + 1 = 8
          const int l = 8;
          String message2 = "";
          for(int x=0;x<l;x++){
            sprintf(str,"%.2X",*((char *)(&ATframe)+x));
            message2 = message2 + str + " ";
          }
          SerialUSB.print("\nAT ");SerialUSB.write(frame.payload[2]);SerialUSB.write(frame.payload[3]);
          SerialUSB.println(" Command received from SMS, forwarding to modem.");
          SerialUSB.println(message2);

          //Send the AT command to the modem.
          digiSerial.write((char*)(&ATframe), l); delay(100);
        }else if(strstr((char *)frame.payload,"CMD:") != 0){
          //Command being sent.
          String cmdFromSMS = "";
          //copy the command to the new string, ignoring the "CMD:" header
          for(int x=0;x<CMDLENGTH;x++){
            cmdFromSMS += (char)frame.payload[x+4];
          }

          //find the command in our list
          int index = findCommandIndex(cmdFromSMS);
          if(index == -1){//command not found
            SerialUSB.print("Command not found: ");
            SerialUSB.println(cmdFromSMS);
          }else{
            SerialUSB.print("Sending command to CANBUS: ");SerialUSB.println(cmdFromSMS);
            txCANBUS(cbCommands[index]);
          }
        }
      }else{
        //other frame types need to be read and ignored I suppose...
      }
    }
#endif
  }

  //toggle one of the LEDs while the xbee is associated
  digitalWrite(DS3,((digitalRead(XBEE_MULT2) == HIGH) ? LOW : HIGH));
}