#include <Arduino.h>
#include <Streaming.h>
#include <algorithm>
#include <array>
#include <cassert>
#include <climits>
#include <cstdint>
#include <cstring>
#include <functional>
#include <string>
#include <string_view>
#include <vector>

using namespace std::literals;

#include <HardwareTimer.h>
#include <chrono>

#define OUTPUT_ARRAYS 1
#define USE_SERIAL 1

#define DEBUG_SERIAL                                                           \
  if (!USE_SERIAL)                                                             \
    ;                                                                          \
  else                                                                         \
    Serial << __FILE__ << "(" << __LINE__ << ")"                               \
           << " " << __func__ << ":"

constexpr int kRadioDataOut = 32;

constexpr uint32_t kBaudRate = 921600;

HardwareTimer *pulse_timer = new HardwareTimer(TIM2);
constexpr uint32_t kTIMOverflowValue = 128 * 1024;
constexpr size_t kPulseBufferSize = 32768;

std::vector<std::pair<bool, uint16_t>> pulse_buffer(kPulseBufferSize);
volatile int pulse_buffer_write_index = 0;

constexpr int kRisingPulseChannel = 1;
constexpr int kFallingPulseChannel = 2;

void PulseCallback(bool rising, int channel) {
  // Store the last capture value to compute difference
  static uint32_t last_capture_value = 0;
  uint32_t capture_value =
      pulse_timer->getCaptureCompare(channel, MICROSEC_COMPARE_FORMAT);
  uint16_t pulse_width = 0;
  if (capture_value > last_capture_value) {
    pulse_width = capture_value - last_capture_value;
  } else {
    pulse_width = (capture_value + last_capture_value) % kTIMOverflowValue;
  }
  pulse_buffer[pulse_buffer_write_index++] = {!rising, pulse_width};
  pulse_buffer_write_index %= kPulseBufferSize;
  last_capture_value = capture_value;
}
inline void RisingPulseCallback() { PulseCallback(true, kRisingPulseChannel); }
inline void FallingPulseCallback() {
  PulseCallback(false, kFallingPulseChannel);
}

void setup() {
  Serial.begin(kBaudRate);
  pinMode(kRadioDataOut, INPUT);
  pulse_timer->setMode(kRisingPulseChannel, TIMER_INPUT_FREQ_DUTY_MEASUREMENT,
                       kRadioDataOut, FILTER_DTS32_N8);
  pulse_timer->setOverflow(kTIMOverflowValue,
                           MICROSEC_FORMAT);
  pulse_timer->attachInterrupt(kRisingPulseChannel, RisingPulseCallback);
  pulse_timer->attachInterrupt(kFallingPulseChannel, FallingPulseCallback);
}

auto PulseToString(std::pair<bool, uint32_t> pulse) -> String {
  String str = "(";
  str += (pulse.first ? "1" : "0");
  str += ",";
  str += pulse.second;
  str += +")";
  return str;
}

void loop() {
  pulse_buffer_write_index = 0;
  pulse_timer->resume();
  delay(2000);
  pulse_timer->pause();
  // Being volatile, it will otherwise reload it every time, but we know it hasn't changed since we turned off interrupts
  int pulse_buffer_write_index_copy = pulse_buffer_write_index;
  auto processing_start = micros();
  if (pulse_buffer_write_index == 0) {
    DEBUG_SERIAL << "No new pulses" << endl;
    return;
  }
  String str;
  for (size_t i = 0; i < pulse_buffer_write_index; ++i) {
    str += PulseToString(pulse_buffer[i]) + ",";
  }
  DEBUG_SERIAL << "Pulses were:[" << str << "]" << endl;
}