//tbv 500SL
#define pos_seq 16
#define pos_txt 25

bool SL500_watchdog_send = false;           //500SL Wachtdog send, thus switched to vectorservice protocol

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

void SL500_generate_code()
{
  //First check if laser is in VectorSevice Protocol
  if (!SL500_watchdog_send) {
    dsp_info("Sending WATCHDOG     ",3);
    Serial.println("Linx 500SL : Sending WatchDog command to switch to VectorService Protocol");

    Linx500SL_Watchdog();
    if (!SL500_watchdog_send) {
      Serial.println("Linx 500SL : Error !! --> No Response from laser");
    }
    if (SL500_watchdog_send) {
      Serial.println("Linx 500SL : Laser switched to VectorService Protocol !!");
    }

    if (!SL500_watchdog_send) {
      dsp_info("No Response laser !!   ",3);
    }
    if (SL500_watchdog_send) {
      dsp_info("Laser Connected !!  ",3);
    }

    delay(5000);
    
    // ik even geen laser heb ;-)
//    SL500_watchdog_send = true;
  }
  else {
    Linx500SL_SendCode();
  }
}



//Send Wachtdog and wait for a reply to switch 500SL laser into VectorService Protocol
void Linx500SL_Watchdog()
{
  char SL500_WD[] = {0xCC, 0xAA, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x13, 0x00, 0x00, 0x00, 0x2B, 0x00, 0x00, 0x00};

  int count = 0;
  byte p2_rec_byte;

  while ((!SL500_watchdog_send) && count < 5) {
    SL500_SEQ++;
    SL500_WD[16] = SL500_SEQ;

    Linx500_Checksum(SL500_WD, 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 == 0xCC) {
      SL500_watchdog_send = true;
    }
    count = count + 1;
    delay(2000);
  }
}


//500SL Send Code
void Linx500SL_SendCode()
{

  char line1[60] = {0xCC, 0xAA, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x37, 0x00, 0x00, 0x00, 0x25, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x000, 0x00};
  char line2[60] = {0xCC, 0xAA, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x01, 0x00, 0x37, 0x00, 0x00, 0x00, 0x25, 0xFF, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x000, 0x00};

  //pos 20 = Remote update Field
  //pos 16 = SEQ
  //pos 25 = Start tekst

  //Generate_Code14();

  //Line 1
  Scroll_Line(line1_14, 14, 0);
  SL500_SEQ++;
  line1[pos_seq] = SL500_SEQ;
  
  dsp_info(line1_14,2);
  Serial.println("");
  Serial.print("Linx500SL --> Line 1 : ");
  for (int i = 0; i <= 13; i++) {
    line1[pos_txt + (i * 2)] = line1_14[i];
    line1[pos_txt + (i * 2) + 1] = 0;
    Serial.write(line1_14[i]);
  }

  Linx500_Checksum(line1, pos_txt + 28);

  line1[pos_txt + 28] = SL500_CRC;
  line1[pos_txt + 28 + 1] = SL500_XOR;

  //Write all chars to Serial port
  for (int i = 0; i <= (pos_txt + 28 + 1); i++) {
    MySerial1.write(line1[i]);
  }
  Serial.println("");

  delay(1000);
  
  //Line 2
  Scroll_Line(line2_14, 14, 1);
  SL500_SEQ++;
  line2[pos_seq] = SL500_SEQ;
  
  dsp_info(line2_14,3);
  Serial.println("");
  Serial.print("Linx500SL --> Line 2 : ");
  for (int i = 0; i <= 13; i++) {
    line2[pos_txt + (i * 2)] = line2_14[i];
    line2[pos_txt + (i * 2) + 1] = 0;
    Serial.write(line2_14[i]);
  }

  Linx500_Checksum(line2, pos_txt + 28);

  line2[pos_txt + 28] = SL500_CRC;
  line2[pos_txt + 28 + 1] = SL500_XOR;

  //Write all chars to Serial port
  for (int i = 0; i <= (pos_txt + 28 + 1); i++) {
    MySerial1.write(line2[i]);
  }
  Serial.println("");
}


//Linx 500 SL Calculate CRC and XOR
void Linx500_Checksum(char *cmd_line, 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 + cmd_line[i];
    val_xor = val_xor ^ cmd_line[i];
  }

  val_crc = 255 - (val_crc % 256);

  Serial.println("");
  Serial.print("Checksum : CRC = ");
  Serial.print(val_crc, HEX);
  Serial.print (" XOR = ");
  Serial.print(val_xor, HEX);
  Serial.println("");

  SL500_CRC = val_crc;
  SL500_XOR = val_xor;
}