#include <CAN.h>
#include <pinout.h>

int i=0;
int s1=0;


void setup()
{
  
  Serial.begin(115200);
  Serial.println("Starting Master...");
  
  CAN.begin(1);
  
  pinMode(3, OUTPUT);
  pinMode(5, OUTPUT);
  digitalWrite(5, LOW);
  digitalWrite(3, LOW);
    
}


void loop()
{

  if(!s1)
  {
    s1=1;
    digitalWrite(3,HIGH);
  }  
  else {
    s1=0;
    digitalWrite(3,LOW);
  }  
  

  CAN_TxMsg.id=0x200;    //Device with speaker sends ID=200, Device with sensor sends=ID=100
  CAN_TxMsg.header.rtr=0;
  CAN_TxMsg.header.length=5;
  CAN_TxMsg.data[0]=s1;
  CAN_TxMsg.data[1]=0x00;
  CAN_TxMsg.data[2]=0x00;
  CAN_TxMsg.data[3]=0x00;
  CAN_TxMsg.data[4]=0x00;
 // CAN_TxMsg.data[5]=0x00;
 // CAN_TxMsg.data[6]=0x00;
 // CAN_TxMsg.data[7]=0x00;

  CAN.send(&CAN_TxMsg);

  if (CAN.CheckNew())
  {

    CAN.ReadFromDevice(&CAN_RxMsg);

    //Print message via Serial Port
    Serial.print(CAN_RxMsg.id,HEX);
    Serial.print(" => ");
    Serial.print(CAN_RxMsg.data[0],HEX);
    Serial.print(" - ");
    Serial.print(CAN_RxMsg.data[1],HEX);
    Serial.print(" - ");
    Serial.print(CAN_RxMsg.data[2],HEX);
    Serial.print(" - ");
    Serial.print(CAN_RxMsg.data[3],HEX);
    Serial.print(" - ");
    Serial.print(CAN_RxMsg.data[4],HEX);
    Serial.print(" - ");
    Serial.println(CAN_RxMsg.data[5],HEX);
//    Serial.print(" - ");
//    Serial.print(CAN_RxMsg.data[6],HEX);
//    Serial.print(" - ");
//    Serial.println(CAN_RxMsg.data[7],HEX);

    //Byte 0 => Info from the other board with the sensor status
    if (CAN_RxMsg.id==0x100 && CAN_RxMsg.data[0])
    {
      digitalWrite(5, HIGH);
    }
    else
      digitalWrite(5, LOW);
 
  }

  delay(1000);


}