byte SL500_SEQ = 00;
byte SL500_CRC = 00;
byte SL500_XOR = 00;


//char SL500SL_WD[] = {204,170,0,0,0,1,0,0,0,1,0,19,0,0,0,43,0,0,0};
char SL500_WD[] = {0xCC,0xAA,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x01,0x00,0x13,0x00,0x00,0x00,0x2B,0x00,0x00,0x00};



//Send Wachtdog and wait for a reply to switch 500SL laser into VectorService Protocol

//Wachdog command CC AA 00 00 00 01 00 00 00 01 00 13 00 00 00 2B [SEQ] [CRC] [XOR]

void Linx500SL_Watchdog()
{
  
  int count = 0;
  byte p2_rec_byte;

  while ((!watchdog_send) && count < 15)
  { 

    SL500_SEQ = SL500_SEQ + 1;
    SL500_WD[16] = SL500_SEQ;

    Linx500_Checksum(16);

    SL500_WD[17] = SL500_CRC;
    SL500_WD[18] = SL500_XOR;
    
    
    Serial.println("Linx 500SL : WatchDog");
    //Send karakters of the watchdog command
    for (int i = 0; i <= 18; i++) 
    {
 //     MySerial1.print(SL500_WD[i], DEC);
        MySerial1.write(SL500_WD[i]);
    }

    //Small delay to give the laser some time to respond
   delay(500);

   if (MySerial1.available() > 0) 
   {
      // read the incoming byte:
      p2_rec_byte = MySerial1.read();
   }
   
   if(p2_rec_byte == 204)
   {
      watchdog_send = true;
   }
    count = count + 1;  
    delay(2000);
  }
}


//500SL Send Code
void Linx500SL_SendCode()
{
  Generate_Code();


  
  
}



void Linx500_Checksum(int char_count)
{

  long val_crc = 0;
  long val_xor = 0;
    
    //Checksum Calculation
    for (int i = 0; i <= char_count; i++) 
    {
      val_crc = val_crc + SL500_WD[i];
      val_xor = val_xor ^ SL500_WD[i];
    }

    val_crc = 255 - (val_crc % 256);

     Serial.print("Checksum : ");
     Serial.println(val_crc);
     Serial.println(val_xor);

    SL500_CRC = val_crc;
    SL500_XOR = val_xor;
}