const char * laser_ranger_error_messages[] =
{
  "VBAT too low, power voltage should be >= 2.0V",
  "Internal error",
  "Module temperature is too low (< -20\'C)",
  "Module temperature is too high (> +40\'C)",
  "Target out of measure range",
  "Invalid measure result",
  "Background light is too strong",
  "Laser signal is too weak",
  "Laser signal is too strong",
  "Hardware fault 1",
  "Hardware fault 2",
  "Hardware fault 3",
  "Hardware fault 4",
  "Hardware fault 5",
  "Laser signal is not stable",
  "Hardware fault 6",
  "Hardware fault 7",
};


void setup()
{
  Serial.begin( 115200 );
  Serial1.begin( 19200 );
  delay( 1000 );
  Serial.println( "Laser RangeFinder Controller" );
}

/*
void sendCommand( char command )
{
  while ( Serial1.available() )
  {
    Serial1.read();
  }

  Serial1.print( command );
}
*/

void loop()
{
  /*static bool waiting_reply = true;
  Serial1.print( 'M' );
  if ( waiting_reply == true )
  {

    while ( Serial1.available() )
    {
      Serial.print( (char)Serial1.read() );
    }
    Serial.println();
  }

  delay( 5000 );*/

  static uint8_t state = 0;
  static bool waiting_reply = false;
  static char reply[32];
  static uint8_t reply_index = 0;
  static uint32_t timeout = 0;

  if ( state == 0 )
  {
    while ( Serial1.available() )
    {
      Serial1.read();
    }

    Serial1.print( 'O' );
    waiting_reply = true;
    timeout = millis();
    state = 1;
  }


  if ( waiting_reply == true )
  {
    if ( timeout != 0 && millis() - timeout >= 2000 )
    {
      waiting_reply = false;
      Serial.println( "timeout, retrying..." );
      timeout = 0;
      state = 0;
    }

    while ( Serial1.available() )
    {
      const char c = Serial1.read();
      Serial.println( (int)c );

      if ( reply_index < sizeof( reply ) - 1 && c != '\r' )
      {
        timeout = 0;
        reply[ reply_index ] = c;
        ++reply_index;
      }
      else
      {
        reply[ reply_index ] = '\0';
        reply_index = 0;
        waiting_reply = false;
        Serial.print( "reply = \"" );
        Serial.print( reply );
        Serial.println( "\"" );
      }
    }
  }
}