#include "message_parser.h"
#include <stdint.h>
#include <string.h>

MessageReceivedCallback on_message_received = NULL;
MessageFailCallback on_message_fail = NULL;

struct MessageReceive InitMessageReceive() {
  struct MessageReceive mr = {
      .state = WaitingForStartByte, // Startzustand des Parsers
      .error = NoError,             // Kein Fehler zu Beginn
      .messageid = 0,               // MSGID auf Standardwert setzen
      // .message Array muss nicht explizit initialisiert werden, da es bei
      // jedem Start geleert wird
      .index = 0,   // Index für das Nachrichten-Array initialisieren
      .checksum = 0 // Checksumme initialisieren
  };
  return mr;
}

// Registrierungsfunktionen für die Callbacks
void register_message_callback(MessageReceivedCallback callback) {
  on_message_received = callback;
}

void register_message_fail_callback(MessageFailCallback callback) {
  on_message_fail = callback;
}

void parse_byte(struct MessageReceive *mr, uint8_t pbyte) {
  switch (mr->state) {
  case WaitingForStartByte:
    if (pbyte == StartByte) {
      mr->index = 0;
      mr->checksum = 0;
      mr->state = GetMessageType;
    }
    break;
  case EscapedMessageType:
    mr->messageid = pbyte;
    mr->checksum ^= pbyte;
    mr->state = InPayload;
    break;
  case GetMessageType:
    if (pbyte == EscapeByte) {
      mr->state = EscapedMessageType;
      return;
    }
    if (pbyte == StartByte || pbyte == EndByte) {
      mr->state = WaitingForStartByte;
      mr->error = UnexpectedCommandByte;
      if (on_message_received) {
        on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
      }
      return;
    }
    mr->messageid = pbyte;
    mr->checksum ^= pbyte;
    mr->state = InPayload;
    break;
  case EscapePayloadByte:
    mr->message[mr->index++] = pbyte;
    mr->checksum ^= pbyte;
    mr->state = InPayload;
    break;
  case InPayload:
    if (pbyte == EscapeByte) {
      mr->state = EscapePayloadByte;
      return;
    }
    if (pbyte == StartByte) {
      mr->state = WaitingForStartByte;
      mr->error = UnexpectedCommandByte;
      if (on_message_received) {
        on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
      }
      return;
    }
    if (pbyte == EndByte) {
      if (mr->checksum != 0x00) {
        // Checksum failure
        // The Checksum gets treated like a normal byte until the end byte
        // accours. Therefore the last byte xor'ed to the checksum ist the
        // checksum so the checksum must be Zero.
        mr->state = WaitingForStartByte;
        mr->error = WrongCheckSum;
        if (on_message_received) {
          on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
        }
        return;
      }
      if (on_message_received) {
        on_message_received(mr->messageid, mr->message,
                            mr->index - 1); // remove checksum byte by just
                                            // setting the length of the message
      }
      mr->state = WaitingForStartByte;
    }
    if (mr->index < MAX_TOTAL_CONTENT_LENGTH) {
      mr->message[mr->index++] = pbyte;
      mr->checksum ^= pbyte;
    } else {
      mr->state = WaitingForStartByte;
      mr->error = MessageToLong;
      if (on_message_received) {
        on_message_fail(mr->messageid, mr->message, mr->index, mr->error);
      }
      return;
    }
    break;
  }
}