#define HWSERIAL Serial1


void setup() {
Serial.begin(9600);
HWSERIAL.begin(19200, SERIAL_8N1);
}

void loop() {
int incomingByte;

if (Serial.available() > 0) {
incomingByte = Serial.read();
Serial.print("USB received: ");
Serial.println(incomingByte, DEC);
//HWSERIAL.print("USB received:");
//HWSERIAL.println(incomingByte, DEC);
}
if (HWSERIAL.available() > 0) {
incomingByte = HWSERIAL.read();
Serial.print("UART received: ");
Serial.println(incomingByte, DEC);
//HWSERIAL.print("UART received:");
//HWSERIAL.println(incomingByte, DEC);
}
}

void LC1::get_AFR() {
  byte incoming_byte;
  byte index;
  byte LC1_status;
  byte LC1_AFR;
  byte LC1_AFRs;
  int LC1_lambda;
  byte packet[8];     

  while (Serial3.available() > 0){
    incoming_byte=Serial3.read(); 
    if ( (incoming_byte & 0xA2) == 0xA2 ) { // synchronization byte
      index=0;
      packet[0]=incoming_byte;
    } 
    else if ((index == 0) && ((incoming_byte & 0x80) == 0x80)) { // 2nd syncro byte
      index=1;
      packet[1]=incoming_byte;
    } 
    else {
      index++;
      packet[index]=incoming_byte;
    }
    if (index >= 5 ) {
      LC1_status = (packet[2] & B00011100) >> 2;

      LC1_AFRs = (packet[2] & B00000001) << 7;
      LC1_AFRs += packet[3] & B01111111;

      LC1_lambda = packet[4] & B00111111;
      LC1_lambda = LC1_lambda << 7;
      LC1_lambda += (packet[5] & B01111111);

      switch (LC1_status) {
      case 0: 
        LC1_AFR = LC1_lambda; 
        break; // reporting lambda
      case 1: 
        LC1_AFR = ((LC1_lambda + 500) * LC1_AFRs ) / 1000; 
        break;
      case 2: 
        Serial.println("Free Air Calibration in progress..."); 
        break;
      case 3: 
        Serial.println("Need Calibration"); 
        break;
      case 4: 
        Serial.println("Heater Warming"); 
        break;
      case 5: 
        Serial.println("Heater Calibration"); 
        break;
      case 6: 
        Serial.println("Error Code"); 
        break;
      case 7: 
        Serial.println("Reserved"); 
        break;
      }
      if (LC1_status == 0 ) {
        Serial.print("lambda ");
        Serial.println(LC1_lambda);
        Serial.print("AFR ");
        Serial.println(LC1_AFR);
        index=0;
        for (byte i; i <=6 ; i++) packet[i]=0;
      }
    }// end index_5
  }//end while
}// end lc1_read

void MSRequest(byte block, unsigned int offset, byte req_bytes) {
#define CAN_ID 3
#define MEGASQUIRT_ID 0
  byte databuffer[8];
  enum msg_type {
    MSG_CMD, MSG_REQ, MSG_RSP, MSG_XSUB, MSG_BURN        };

  databuffer[0]=block;
  databuffer[1]=offset >> 3;
  databuffer[2]=(((offset & B00000111) << 5 ) | req_bytes);

  MS_tx_packet(offset, MSG_REQ, CAN_ID, MEGASQUIRT_ID, block, 3, databuffer);
}


void MS_tx_packet(unsigned int var_offset, byte msg_type, byte from_id, byte to_id, byte var_block, byte dlc, byte data[8]) {
  //Spare bits unused
  byte IDR0, IDR1, IDR2, IDR3;

  /* Megasquirt CANBus Format -> FlexCan decoder
   .          76543210
   .   IDR0 - ___ooooo - 3 bits blank, offset
   .   IDR1 - oooooomm - offset, msg_type
   .   IDR2 - mffffttt - msg_type, from_id, to_id
   .   IDR3 - tbbbbBss - to_id, block, spare
   .   Flexcan msg.id
   .            |   IDR0  |   IDR1  |   IDR2  |   IDR3  |   
   .   can.id  - 0001 1111 1111 1111 1111 1111 1111 1111
   .   overlay - ___o oooo oooo oomm mfff fttt tbbb bBss
   */

  // Cut n Paste bits into bytes
  //    ___ooooo - first 3 bits truncated in the can.id long
  IDR0 = lowByte((var_offset >> 6)); // shift right, then paste

  //    oooooomm - offset, msg_type
  IDR1 = (lowByte(var_offset) & B00111111) << 2;
  IDR1 += ((msg_type & B00000110) >> 1);

  //    mffffttt - msg_type, from_id, to_id
  IDR2 = ((msg_type & B00000001) << 7);
  IDR2 += (from_id & B00001111) << 3;
  IDR2 += (to_id &  B00001110) >> 1;
  //    tbbbbBss - to_id, block, spare
  IDR3 = (to_id & B00000001) << 7;
  IDR3 += (var_block & B00001111) << 3;
  IDR3 += (var_block & B00010000) >> 2;

  msg.id=0; // clear data
  // assemble bytes into uint32 for msg.id
  msg.id += IDR0 << 24;
  msg.id += IDR1 << 16;
  msg.id += IDR2 << 8;
  msg.id += IDR3;
  msg.ext = 1; // extended ID set
  msg.len = dlc;

  for ( byte idx=0; idx<8; ++idx ) {
    msg.buf[idx] = data[idx];
  }


#ifdef DEBUG
  Serial.print("var_offset: ");
  Serial.println(var_offset);
  Serial.print("msg_type: ");
  Serial.println(msg_type);
  Serial.print("from_id: ");
  Serial.println(from_id);
  Serial.print("to_id: ");
  Serial.println(to_id);
  Serial.print("var_block: ");
  Serial.println(var_block);
  Serial.print("dlc: ");
  Serial.println(dlc);
  Serial.println("msg.id: ");
  Serial.println(msg.id, HEX);
#endif

  CANbus.write(msg);
}