#include "public/gonk/fpga_control.h"
#include <Arduino.h>
#include <bitset>
#include <cstddef>
#include <stdint.h>

#include "gonk/fpga_control.h"

#define PW_LOG_LEVEL PW_LOG_LEVEL_DEBUG
#define PW_LOG_MODULE_NAME "FpgaControl"

#include "pw_log/log.h"
#include "pw_result/result.h"
#include "pw_span/span.h"
#include "pw_status/status.h"

using gonk::pin_config::FlashCLK;
using gonk::pin_config::FlashCS;
using gonk::pin_config::FlashMISO;
using gonk::pin_config::ICE40Done;
using gonk::pin_config::PinConfig;

namespace gonk::fpga_control {

FpgaControl::FpgaControl(PinConfig *pin_config) : pin_config_(pin_config) {}

Status FpgaControl::StartConfig() {
  bitstream_file_position = 0;
  memset(read_buffer, 0, 4);
  memset(bitstream_file, 0, BitstreamFileLength);
  bytes_written = 0;

  Status wait_result = WaitForBitstreamStart();
  if (!wait_result.ok()) {
    return wait_result;
  }
  ReadBitstream();
  auto verify_result = VerifyBitstream();
  if (!verify_result.ok()) {
    return verify_result;
  }
  return SendBitstreamToFpga();
}

Status FpgaControl::WaitForBitstreamStart() {
  uint32_t last_update = millis();
  uint32_t this_update = millis();

  const uint32_t update_interval_ms = 1000;

  while (true) {
    // Output a heartbeat message while waiting for data.
    this_update = millis();
    if (bytes_written == 0 && this_update > last_update + update_interval_ms) {
      last_update = this_update;
      PW_LOG_INFO("Waiting for bitstream");
    }

    // If no data waiting start over.
    if (!Serial.available()) {
      continue;
    }

    uint8_t data_in = Serial.read();
    read_buffer[bytes_written % 4] = data_in;
    bytes_written++;

    PW_LOG_INFO("Discard byte: %d", bytes_written);
    // If the last 4 bytes recieved match the start sequence, break.
    if (read_buffer[(bytes_written + 0) % 4] == 0xff &&
        read_buffer[(bytes_written + 1) % 4] == 0x00 &&
        read_buffer[(bytes_written + 2) % 4] == 0x00 &&
        read_buffer[(bytes_written + 3) % 4] == 0xff) {
      PW_LOG_INFO("Start sequence found.");
      break;
    } else if (bytes_written >= 32) {
      // If the start sequence hasn't been found in the last 32 bits
      // something likely went wrong. Abort and restart.
      PW_LOG_ERROR("Start sequence not found.");
      return pw::Status::Aborted();
    }
  }

  return pw::OkStatus();
}

Status FpgaControl::ReadBitstream() {
  char bitstream_buffer[4096];

  while (true) {
    size_t read_byte_count = Serial.readBytes(bitstream_buffer, 4096);
    PW_LOG_INFO("Got bytes: %d", read_byte_count);
    bytes_written += read_byte_count;

    memcpy(&bitstream_file[bitstream_file_position], bitstream_buffer,
           read_byte_count);
    bitstream_file_position += read_byte_count;

    if (bytes_written >= 135100) {
      PW_LOG_INFO("All 135100 bytes recieved.");
      break;
    }
  }

  return pw::OkStatus();
}

Status FpgaControl::VerifyBitstream() {
  // TODO(tonymd): Verify the bitstream with somehow, perhaps move to pw_hdlc.
  PW_LOG_INFO("File ready: %d", bitstream_file_position);

  PW_LOG_INFO("First 12 bytes");
  for (int i = 0; i < 12; i++) {
    PW_LOG_INFO("%x", bitstream_file[i]);
  }

  PW_LOG_INFO("Last 12 bytes");
  for (int i = bitstream_file_position - 12; i < bitstream_file_position; i++) {
    PW_LOG_INFO("%x", bitstream_file[i]);
  }

  return pw::OkStatus();
}

Status FpgaControl::SendBitstreamToFpga(const uint32_t config_timeout_ms) {
  PW_LOG_INFO("Sending bitstream file to the FPGA.");

  pin_config_->SPIDisable();
  pin_config_->FpgaSpiConfigMode();

  digitalWrite(FlashCS, LOW);

  // Write out the bitstream file similar to an SPI transaction but using MISO
  // as the output pin.
  for (int i = 0; i < bitstream_file_position; i++) {
    uint8_t d = bitstream_file[i];
    // Write out each 8 bits.
    for (int b = 0; b < 8; b++) {
      if (d & 0x80) {
        digitalWrite(FlashCLK, LOW);
        digitalWrite(FlashMISO, HIGH);
        digitalWrite(FlashCLK, HIGH);
      } else {
        digitalWrite(FlashCLK, LOW);
        digitalWrite(FlashMISO, LOW);
        digitalWrite(FlashCLK, HIGH);
      }
      d <<= 1;
    }
    digitalWrite(FlashCLK, HIGH);
  }

  digitalWrite(FlashCS, HIGH);

  uint32_t last_update = millis();
  uint32_t this_update = millis();
  bool done = false;
  int ice40_done = 0;

  // Wait for ICE40Done (CDONE) signal to go high
  while (!done) {
    // Write one clock pulse
    digitalWrite(FlashCLK, LOW);
    delayMicroseconds(100);
    digitalWrite(FlashCLK, HIGH);
    delayMicroseconds(100);

    ice40_done = digitalRead(ICE40Done);
    if (ice40_done == 1) {
      PW_LOG_INFO("FPGA Config Success.");
      break;
    }

    this_update = millis();
    // If more than two seconds have passed something likely went wrong.
    if (this_update > last_update + config_timeout_ms) {
      PW_LOG_ERROR("FPGA Config failed.");
      last_update = this_update;
      return pw::Status::Aborted();
    }
  }

  // Send 50 more clock pulses to release release the SPI bus to user control.
  for (int i = 0; i < 50; i++) {
    digitalWrite(FlashCLK, LOW);
    delayMicroseconds(100);
    digitalWrite(FlashCLK, HIGH);
    delayMicroseconds(100);
  }

  // Re-enable SPI
  pin_config_->SPIEnable();

  return pw::OkStatus();
}

} // namespace gonk::fpga_control