#include <HardwareSerial.h>

HardwareSerial MySerial1(1);
HardwareSerial MySerial2(2);

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define pin_LASERTYPE   12
#define pin_MODE  13
#define pin_RXD1  5
#define pin_TXD1  18
#define pin_RXD2  16
#define pin_TXD2  17

long now = 0;
long prev = 0;
int incomingByte = 0;                 // for incoming serial data
bool b_mode;
bool b_type;
int lasertype = 0;                    //Lasertype selected
//tbv 500SL
bool watchdog_send = false;           //500SL Wachtdog send, thus switched to vectorservice protocol

//void Linx500SL_Watchdog();
//void Linx500SL_SendCode();

void setup() {


  pinMode(pin_LASERTYPE, INPUT_PULLUP);
  pinMode(pin_MODE, INPUT_PULLUP);

  //init debug port  
  Serial.begin(9600);

  if(digitalRead(pin_LASERTYPE) == false)
  {
    Serial.println("Lasertype set to : Linx 500SL - baudrate 19k2");
    //init serial ports
    MySerial1.begin(19200, SERIAL_8N1, pin_RXD1, pin_TXD1);
    MySerial2.begin(19200, SERIAL_8N1, pin_RXD1, pin_TXD2);
    lasertype = 1;
  } 
  else
  {
    Serial.println("Lasertype set to : Linx ??? - baudrate 9600");
    //init serial ports
    MySerial1.begin(9600, SERIAL_8N1, pin_RXD1, pin_TXD1);
    MySerial2.begin(9600, SERIAL_8N1, pin_RXD1, pin_TXD2);
    lasertype = 2;
  }

  //init I2C Display
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Clear the buffer
  display.clearDisplay();
  display.setTextSize(1);             
  display.setTextColor(SSD1306_WHITE);  
  display.setCursor(10,0);            
  display.println(F("RS232 Sniffer v1.0"));
  delay(1000);
  display.display();
  display.clearDisplay();
  display.setCursor(10,0);            
  if(lasertype==1) {display.println(F("Linx 500SL"));}
  if(lasertype==2) {display.println(F("MundiScan 2000"));}    
  display.setCursor(0,10);            
  display.println(F("Sniffer mode selected"));
  display.display();
  delay(2000);

}


void loop() {
  // put your main code here, to run repeatedly:

  if(digitalRead(pin_MODE) != b_mode)
  {
     b_mode = digitalRead(pin_MODE);
     if(b_mode)
     {
       Serial.println("Mode : Sniffer");
       display.clearDisplay();
       display.setTextSize(1);             
       display.setTextColor(SSD1306_WHITE);  
       display.setCursor(10,0);            
       if(lasertype==1) {display.println(F("Linx 500SL"));}
       if(lasertype==2) {display.println(F("MundiScan 2000"));}    
       display.setCursor(0,10);            
       display.println(F("Sniffer mode selected"));
       display.display();
       delay(1000);
     }
     else
     {
       Serial.println("Mode : Generate code");
       display.clearDisplay();
       display.setTextSize(1);             
       display.setTextColor(SSD1306_WHITE);  
       display.setCursor(10,0);            
       if(lasertype==1) {display.println(F("Linx 500SL"));}
       if(lasertype==2) {display.println(F("MundiScan 2000"));}    
       display.setCursor(0,10);            
       display.println(F("Send mode selected"));
       display.display();
       delay(1000);
     }
  
    
  }

  if(b_mode)
  {
   if (MySerial1.available() > 0) {
    // read the incoming byte:
    incomingByte = MySerial1.read();

    // say what you got:
    Serial.print("P1 received: ");
    Serial.println(incomingByte, DEC);
  }

    if (MySerial2.available() > 0) {
      // read the incoming byte:
      incomingByte = MySerial2.read();

      // say what you got:
      Serial.print("P2 received: ");
      Serial.println(incomingByte, DEC);
    }
  }

  //generating mode
  else
  {
    now = millis();
    if(now - prev >= 5000)
    {

      //Laser 500SL
      if(lasertype == 1)
      {
        if(!watchdog_send)
        {
           display.setCursor(0,20);  
           
           display.setTextColor(WHITE, BLACK);
          
           display.println(F("Sending WATCHDOG       "));
           display.display();
   
          Serial.println("Linx 500SL : Sending WatchDog command to switch to VectorService Protocol");
        
          Linx500SL_Watchdog();
          if(!watchdog_send) {Serial.println("Linx 500SL : Error !! --> No Response from laser");}
          if(watchdog_send) {Serial.println("Linx 500SL : Laser switched to VectorService Protocol !!");} 

       display.setCursor(0,20);            
       display.setTextColor(WHITE, BLACK);
       if(!watchdog_send) {display.println(F("No Response laser !! "));}
       if(watchdog_send) {display.println(F("Laser Connected !!  "));}    
          display.display();
          delay(5000);
 
        }
        else
        {
          Linx500SL_SendCode();
        }
      }

      if(lasertype == 2)
      {
        Serial.println(".");
        MySerial1.println("P1 : Hallo Frank");
        MySerial2.println("P2 : Hallo Frank");
      }
      
      prev = now;
    }

   
 
  }
}