//revisione 2 del 30/03/2018, con Arduino 1.8.5
/*
  CAN BUS + ARDUINO --> CANINO interface :)
  USB <--> CAN BUS interface
  Author: Luigi Rubino

  Adesso accetta 3 byte di indirizzo

  STRING EXAMPLE:
  @00A0102030405060708\n
*/

#include "mcp_can.h"
#include <SPI.h>

#define CAN0_INT 2                              // Set INT to pin 2
MCP_CAN CAN0(10);                               // Set CS to pin 10


// the cs pin of the version after v1.1 is default to D9
// v0.9b and v1.0 is default D10
// Can_Bus_shield-ISO-v1 is default D10
//const int SPI_CS_PIN = 10;
//MCP_CAN CAN(SPI_CS_PIN);                                    // Set CS pin

unsigned char buf[8];
unsigned char canId;
long unsigned int rxId;


#define LEN_STRING 10

unsigned char ReceivedString[LEN_STRING];  //stringa ricezione carattere da USB

int inByte = 0;         // incoming serial byte

//global var
int  digValue, sign=1;
char rxpos, rxpos2;          //per il parser seriale
int address;                //comando da eseguire
int DT;


void setup() {
  // start serial port at 115200 bps:
  Serial.begin(115200); //provare velocità 115200
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }
 
  // Initialize MCP2515 running at 16MHz with a baudrate of 500kb/s and the masks and filters disabled.
  if(CAN0.begin(MCP_ANY, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
    Serial.println("MCP2515 Initialized Successfully!");
  else
    Serial.println("Error Initializing MCP2515...");
  
  CAN0.setMode(MCP_NORMAL);                     // Set operation mode to normal so the MCP2515 sends acks to received data.

  pinMode(CAN0_INT, INPUT);                            // Configuring pin for /INT input
  
  Serial.println("CANINO READY...");
}

void loop() {

  unsigned char len = 0;
  
  // if we get a valid byte, read analog ins:
  if (Serial.available() > 0) {
    // get incoming byte:
    inByte = Serial.read();
    //Serial.write(inByte);
    // delay 10ms to let the ADC recover:
    //delay(10);
    
 /* Get the character received from the USART */
  if (inByte=='@')
   {
     rxpos     = 1;
     digValue  = 0;
   }
  else
  {
    
//    Serial.print("-->");
//    Serial.print((int) rxpos);
//    Serial.println();
    
  switch(rxpos)
  {
   case 1:
      address   = hex2int(inByte);
      rxpos    += 1;
      
      break;

   case 2:
      address   <<=4; 
      address += hex2int(inByte);     
      rxpos    += 1;

      break;
      
   case 3:
      address   <<=4; 
      address += hex2int(inByte);    
      rxpos    += 1;
      rxpos2   =  0; 

      break;
                  
   case 4:   //byte 1
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 5:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;

   case 6:   //byte 2
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 7:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;
 
   case 8:   //byte 3
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 9:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;

   case 10:   //byte 4
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 11:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;
      
   case 12:   //byte 5
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 13:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;

   case 14:   //byte 6
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 15:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;

   case 16:   //byte 7
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 17:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;

   case 18:   //byte 8
      DT = hex2int(inByte);
      rxpos    += 1;
      break;
   case 19:
      DT   <<=4; 
      DT += hex2int(inByte);
      ReceivedString[rxpos2] = DT;
      rxpos    += 1;
      rxpos2   += 1;
      break;
                  
   case 20: //eseguo il comando
        CAN0.sendMsgBuf(address, 0, 8, ReceivedString);
        //byte sndStat = CAN0.sendMsgBuf(0x100, 0, 8, data);
    //delay(100);          // send data per 100ms
      rxpos    = 0;

//da rinuovere
//      Serial.print(address, HEX);
//      for (int kk=0;kk<=7; kk++)
//      {
//        Serial.print(" ");
//        Serial.print(ReceivedString[kk], HEX);
//        
//        }
//        Serial.println();
    break;
    default: 
        rxpos    = 0;
        break;
  }
  }    //fine gestione acquisizione 
  }  //fine gestione seriale

  // --------------------------------------------------
  //gestione CAN BUS
  if(!digitalRead(CAN0_INT))                         // If CAN0_INT pin is low, read receive buffer
  {
    CAN0.readMsgBuf(&rxId, &len, buf);      // Read data: len = data length, buf = data byte(s)

    canId = (unsigned char) rxId;
//    canId = CAN0.getCanId();       // indirizzo risposta
        
 //       Serial.println("-----------------------------");
 //       Serial.print("Get data from ID: ");
        Serial.print("@");
        Serial.print(canId, HEX);

        for(int i = 0; i<len; i++)    // print the data
        {
            if (buf[i]<=15)  {  Serial.print('0');}
            Serial.print(buf[i], HEX);
        }
        Serial.println();
    }
 
}

void establishContact() {
  while (Serial.available() <= 0) {
    Serial.print('A');   // send a capital A
    delay(300);
  }
}

int hex2int (char c)
{
  if (c >= '0' && c <= '9')
  {
      return (int) (c-'0');
  }
  else
  {
      return (int) ((c - 'A')+10);
  }
}