#include "message_builder.h"
#include "esp_log.h"
#include "message_parser.h"
#include <stdbool.h>
#include <stddef.h>

bool needs_stuffing_byte(uint8_t byte) {
  return (byte == StartByte || byte == EscapeByte || byte == EndByte);
}

bool add_byte_with_length_check(uint8_t byte, size_t write_index, uint8_t *data,
                                size_t max_length) {
  if (write_index >= max_length) {
    return false;
  }
  data[write_index] = byte;
  return true;
}

int build_message(uint8_t msgid, const uint8_t *payload, size_t payload_len,
                  uint8_t *msg_buffer, size_t msg_buffer_size) {

  ESP_LOGE("BM", "payload_len %d, msg_buffer_size %d", payload_len + 4,
           msg_buffer_size);
  if (payload_len + 4 > msg_buffer_size) {
    return PayloadBiggerThenBuffer;
  }

  uint8_t checksum = 0;
  size_t write_index = 0;
  msg_buffer[write_index++] = StartByte;

  if (needs_stuffing_byte(msgid)) {
    if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
                                    msg_buffer_size)) {
      return BufferOverFlow;
    }
    write_index++;
  }
  if (!add_byte_with_length_check(msgid, write_index, msg_buffer,
                                  msg_buffer_size)) {
    return BufferOverFlow;
  }
  write_index++;
  checksum ^= msgid;

  for (size_t i = 0; i < payload_len; i++) {
    if (needs_stuffing_byte(payload[i])) {
      if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
                                      msg_buffer_size)) {
        return BufferOverFlow;
      }
      write_index++;
    }
    if (!add_byte_with_length_check(payload[i], write_index, msg_buffer,
                                    msg_buffer_size)) {
      return BufferOverFlow;
    }

    write_index++;
    checksum ^= payload[i];
  }

  if (needs_stuffing_byte(checksum)) {
    if (!add_byte_with_length_check(EscapeByte, write_index, msg_buffer,
                                    msg_buffer_size)) {
      return BufferOverFlow;
    }
    write_index++;
  }

  if (write_index + 2 > msg_buffer_size) {
    return BufferOverFlow;
  }
  msg_buffer[write_index++] = checksum;
  msg_buffer[write_index++] = EndByte;

  ESP_LOGE("BM", "MESSAGE FERTIG GEBAUT");
  return write_index;
}