#define BUFLEN      512
#define SERIALSPEED 115200
#define ERROR_SYNTAX  1
#define ERROR_COMMAND 2

typedef struct CmdParser {
  char buffer[BUFLEN] = {0};
  char atoibuf[32] = {0};
  char *tok = NULL;
  char *atoiptr = NULL;
  int pin = 0;
} CmdParser;

void setup() {
  Serial0.begin(SERIALSPEED, SERIAL_8N1, 21, 14);
}

int parse_digit_from_buffer(CmdParser *parser) 
{
  parser->atoiptr = parser->tok;
  while ( *parser->atoiptr != ' ' && *parser->atoiptr != 0x00 ) {
    if ( !isDigit((char)*parser->atoiptr) ) { 
      Serial0.printf("Expected digit, found %s\n", parser->tok);
      return ERROR_SYNTAX;
    } else if (  parser->atoiptr == ((char *)&parser->buffer + 512 ) ) {
      Serial0.printf("Reached end of string looking for next token : %s\n", parser->tok);
      return ERROR_SYNTAX;
    }
    parser->atoiptr++;
  }
  strncpy((char *)&parser->atoibuf, parser->tok, (int)(parser->atoiptr - parser->tok));
  parser->tok = parser->atoiptr;
  return 0;
}

int command_writepin(CmdParser *parser)
{
  int rc = 0;
  parser->tok = strtok(NULL, " ");
  rc = parse_digit_from_buffer(parser);
  if ( rc != 0 ) {
    return rc;
  }
  parser->pin = atoi((char *)&parser->atoibuf);
  parser->tok = strtok(NULL, " ");
  if ( strncmp(parser->tok, "high", 4) == 0 ) {
    digitalWrite(parser->pin, HIGH);
    Serial0.printf("digitalWrite(%d, HIGH)", parser->pin);
  } else if ( strncmp(parser->tok, "low", 3) == 0 ) {
    digitalWrite(parser->pin, LOW);
    Serial0.printf("digitalWrite(%d, LOW)", parser->pin);
  } else {
    Serial0.printf("Expected HIGH or LOW, found %s", parser->tok);
    return ERROR_SYNTAX;
  }
  return 0;
}

int command_setpin(CmdParser *parser)
{
  int rc = 0;
  parser->tok = strtok(NULL, " ");
  rc = parse_digit_from_buffer(parser);
  if ( rc != 0 ) {
    return rc;
  }
  parser->pin = atoi((char *)&parser->atoibuf);
  parser->tok = strtok(NULL, " ");
  if ( strncmp(parser->tok, "input", 5) == 0 ) {
    pinMode(parser->pin, INPUT);
    Serial0.printf("pinMode(%d, INPUT)", parser->pin);
  } else if ( strncmp(parser->tok, "output", 6) == 0 ) {
    pinMode(parser->pin, OUTPUT);
    Serial0.printf("pinMode(%d, OUTPUT)", parser->pin);
  } else {
    Serial0.printf("Expected input or output, found %s", parser->tok);
    return ERROR_SYNTAX;
  }
  return 0;
}

int parse_command(CmdParser *parser)
{
  parser->tok = strtok((char *)&parser->buffer, " ");
  if ( strcmp(parser->tok, "setpin") == 0 ) {
    return command_setpin(parser);
  } else if ( strcmp(parser->tok, "writepin") == 0 ) {
    return command_writepin(parser);
  }
  return ERROR_COMMAND;
}

void loop() {
  CmdParser parser;
  memset((void *)&parser, 0x00, sizeof(CmdParser));
  Serial0.println("");
  Serial0.println("READY");
  while ( Serial0.available() == 0 ) {
    delay(10);
  }
  if ( Serial0.readBytesUntil('\0', (char *)&parser.buffer, BUFLEN) != 0 ) {
    switch ( parse_command(&parser) ) {
      case ERROR_SYNTAX:
        Serial0.println("Syntax error");
        break;
      case ERROR_COMMAND:
        Serial0.println("Unknown command");
        break;
      case 0:
        return;
    }
  }
}