#include <SPI.h>          //SPI is used to talk to the CAN Controller
#include <mcp_can.h>
#include <defaults.h> // added
#include <global.h>   // added
const int buzzerPin = 8; //Led is buzzer connected to pin 8 added

MCP_CAN CAN(10);          //set SPI Chip Select to pin 10

unsigned char len = 0;
unsigned char buf[8];
unsigned int canID;


//******************************** Setup *********************************//

void setup()
{
  Serial.begin(115200);   //to communicate with Serial monitor
  pinMode(buzzerPin, OUTPUT); //Set digital pin 8 to OUTPUT for buzzer
 

//tries to initialize, if failed --> it will loop here for ever
START_INIT:

    if(CAN_OK == CAN.begin(CAN_500KBPS))      //setting CAN baud rate to 500Kbps
    {
        Serial.println("CAN BUS Shield init ok!");
    }
    else
    {
        Serial.println("CAN BUS Shield init fail");
        Serial.println("Init CAN BUS Shield again");
        nosound();
        delay(100);
        goto START_INIT;
    }
}

void sound1() // for distance 1
{
          digitalWrite(buzzerPin, HIGH); // problemem jest delay przytyka działanie jak wykożystać millis?
          delay(500);                             
          digitalWrite(buzzerPin, LOW); //
}

void sound2() // for distance 2
{
          digitalWrite(buzzerPin, HIGH); //Set buzzer on
          delay(300);
          digitalWrite(buzzerPin, LOW); //Set buzzer off
}

void sound3() // for distance 3
{
          digitalWrite(buzzerPin, HIGH); //Set LED on
          delay(100);
          digitalWrite(buzzerPin, LOW); //Set LED off
}

void sound4() // for distance 4
{
          digitalWrite(buzzerPin, HIGH); //Set buzzer on
          delay(50);
          digitalWrite(buzzerPin, LOW); //Set buzzer off
}

void nosound() // for stop buzzer
{
                    digitalWrite(buzzerPin, LOW); //Set buzzer off
}

void loop()


{
    if(CAN_MSGAVAIL == CAN.checkReceive())        //check if data coming
    {
        CAN.readMsgBuf(&len, buf);    //read data,  len: data length, buf: data buffer
        canID = CAN.getCanId();       //getting the ID of the incoming message
        CAN.init_Mask(0, 0, 0x200);
       
        if (canID == 0x200)            //reading only our beloved 0x597 message
        {
          Serial.print("ID is: ");
          Serial.print(canID, HEX);     //printing the ID in its standard form, HEX

          Serial.print("    Length is: ");
          Serial.println(len);

          Serial.println("\n\n");
         
           
         

          // ** distance signal read
         
          if((buf[0] == 0x40) )
          {
            Serial.println("Byte 0 Sygnał odległości : 1 +");
            sound1();
           
          }

          if((buf[0] == 0x41) )
          {
            Serial.println("Byte 0 Sygnał odległości 2 : ++");
            sound2();
           
          }

          if((buf[0] == 0x42) )
          {
            Serial.println("Byte 0 Sygnał odległości 3 : +++");
            sound3();
          }

         
          if((buf[0] == 0x43) )
          {
            Serial.println("Byte 0 Sygnał odległości 3 : +++");
            sound4();
          }

          else if((buf[0] | 0x40, 0x41, 0x42, 0x43) )
          {
            Serial.println("no data");
            nosound();
          }
         
         }
      }
}