#include <stdio.h>
#include <SoftwareSerial.h>

unsigned long maxTimeout = 20; //20 mS timeOut
int speed;

SoftwareSerial mySerial(10, 11);

void setTimeOut(unsigned long n){
  maxTimeout = n;
}

unsigned long getTimeOut(void){
  return maxTimeout;
}

int serial_read(uint8_t *dataStream, int size){
  int count = 0;
  unsigned long timeOut = getTimeOut() + millis(); //Actual time
  
  while((millis() < timeOut) && (count < size)){// Wait until data recived and no timeout
    if(mySerial.available()){
      dataStream[count]=mySerial.read();
      count++;
    }
    yield();
  }
  return count;                                   
}

void setup() {
  mySerial.begin(19200);
  Serial.begin(115200, SERIAL_8N1);
  pinMode(13, OUTPUT);
}

void loop() {
  uint8_t comm1[]={0x03, 0xC0, 0x59, 0xC2, 0x00, 0x92, 0x42};
  uint8_t resp[10];
  char cad[50], *p;
  uint8_t ack[]={0xCA};
  int readed, i=0;

  p = cad;
  readed = serial_read(resp, 6);
  if((readed == 6) && (resp[0]==0x02)&&(resp[1]==0x1D)&&(resp[2]==0x58)&&
      (resp[3]==0xC2)&&(resp[4]==0x92)&&(resp[5]==0x42)){
    mySerial.write(0x1A);
    digitalWrite(13, HIGH);
    for(i=0; i<7; i++){
      mySerial.write(comm1[i]);
    }
    readed += serial_read(&resp[readed], 1); //ACK
    digitalWrite(2, LOW);
  }
  for(i=0; i<readed; i++){
    snprintf(p, 50, "%02X", resp[i]);
    p+=2;
  }
  if(readed > 0){
    sprintf(p, "\n");
    Serial.write(cad);
  }
}