#include "SerialConnect.h"

SerialConnect::SerialConnect(int id_size)
{
  _id_size = id_size; //number of digits in identifier for command
}

void SerialConnect::init( )
{
  _positionInBuffer = 0;
  _last_packet_time = millis();
}

void SerialConnect::update(){
  readSerial();
  if (millis()-_last_packet_time > _packet_interval){
    Serial.println(this->_packet_func()); _last_packet_time = millis();
  }
}

void SerialConnect::readSerial() {
  if (Serial.available() > 0) {
    _buffer[_positionInBuffer] = (char) Serial.read();
    if (_buffer[_positionInBuffer] == '\n') {
      // done reading
      _buffer[++_positionInBuffer] = '\0';
      _positionInBuffer = 0;
      this->onSerialRead(_buffer);
    } else {
      _positionInBuffer++;
    }
  }
}

void SerialConnect::onSerialRead(char *in) {
  String command = String(in);
  int id = command.substring(0,_id_size).toInt();
  int val = command.substring(_id_size+1).toInt();
  this->_handle_command_func(id,val);
}