// The ChronoPulse: board side (Seeed XIAO RP2040 on the QPAD-xiao board)
// Fadi G. Haddad, HTMAA 2026. Pair with health_monitor.py running on a laptop.
// Built on Quentin Bolsee's computer_link example (qpad-xiao, MIT License):
// https://forge.cba.mit.edu/quentin.bolsee/qpad-xiao

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Adafruit_NeoPixel.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET     -1
#define SCREEN_ADDRESS 0x3C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET, 1700000UL, 1700000UL);

#define HDLC_FLAG 0x7E
#define HDLC_ESC 0x7D
#define HDLC_FLAG_ENC 0x5E
#define HDLC_ESC_ENC 0x5D

#define LENGTH_BUFFER 1024

#define N_TOUCH 6
#define THRESHOLD 6

uint8_t* buffer_pointer;

#define LENGTH_BUFFER_INPUT 2500

char buffer_in[LENGTH_BUFFER_INPUT];

// touch state
int touch_pins[N_TOUCH] = {3, 4, 2, 27, 1, 26};
int touch_values[N_TOUCH] = {0, 0, 0, 0, 0, 0};
uint8_t touch_state = 0;

// three single-color LEDs, wired active-low: HIGH = off, LOW = on
#define PIN_RED 17
#define PIN_GREEN 16
#define PIN_BLUE 25

// middle full-color LED (PIN_NEOPIXEL = 12, NEOPIXEL_POWER = 11, defined by the board core)
Adafruit_NeoPixel pixel(1, PIN_NEOPIXEL, NEO_GRB + NEO_KHZ800);

// health states
enum Health { FINE, CAUTION, DANGER, OFF };
Health health = OFF;  // start on the title page: lights off, logo view

// test mode: cycle through all three states automatically
#define AUTO_CYCLE false
#define STATE_DURATION_MS 8000

// what the laptop should draw on the screen
enum View { WAVES, LOGO };
View view = LOGO;

// directional touch pads (indices into touch_pins)
#define PAD_DOWN  2
#define PAD_LEFT  3
#define PAD_RIGHT 4
#define PAD_UP    5

// view touch pads
#define PAD_WAVES 0
#define PAD_LOGO  1

// screensaver: switch the screen off after this long with no touches (protects against burn-in)
#define SCREENSAVER_MS 60000
unsigned long last_touch_ms = 0;
bool screen_asleep = false;
bool wake_touch_held = false;  // true while the finger that woke the screen is still down

void update_touch() {
  int t;
  int t_max = 200;
  int p;

  touch_state = 0;
  for (int i = 0; i < N_TOUCH; i++) {
    p = touch_pins[i];

    // set to low
    pinMode(p, OUTPUT);
    digitalWriteFast(p, LOW);

    // settle
    delayMicroseconds(25);

    // enable pull-up
    pinMode(p, INPUT_PULLUP);

    // measure time to rise
    t = 0;
    while (!digitalReadFast(p) && t < t_max) {
      t++;
    }
    touch_values[i] = t;

    // update state
    touch_state |= (touch_values[i] > THRESHOLD) << i;
  }
}

// every light shares the same resting glow; each has its own peak
#define GLOW_BASE 0.25
#define MIDDLE_PEAK 0.4
#define SINGLES_PEAK 1.0

// maps a pulse wave (0..1) onto brightness between GLOW_BASE and `peak`
float shape(float wave, float peak) {
  if (wave > 1.0) wave = 1.0;
  if (wave < 0.0) wave = 0.0;
  return GLOW_BASE + (peak - GLOW_BASE) * wave;
}

// r, g, b: the state's color. wave: the pulse shape, 0..1
// drives the middle light and the three single lights from the same wave, so they stay in sync
void setPixel(int r, int g, int b, float wave) {
  float mid = shape(wave, MIDDLE_PEAK);
  float sgl = shape(wave, SINGLES_PEAK);

  // gamma correction: re-bends values so fades look even to the eye
  pixel.setPixelColor(0, pixel.Color(pixel.gamma8(r * mid),
                                     pixel.gamma8(g * mid),
                                     pixel.gamma8(b * mid)));
  pixel.show();

  // single lights are active-low, so the value is inverted
  analogWrite(PIN_RED,   255 - pixel.gamma8(r * sgl));
  analogWrite(PIN_GREEN, 255 - pixel.gamma8(g * sgl));
  analogWrite(PIN_BLUE,  255 - pixel.gamma8(b * sgl));
}

// ---------------------------------------------------------------------------
// Heart-monitor waves. These MUST match the same functions in health_monitor.py,
// so the trace on screen and the lights stay in sync.
// Each returns roughly -1..1 (the trace); led_wave() turns that into 0..1 brightness.
// ---------------------------------------------------------------------------

// a smooth bump centered at `center` ms, about `width` ms wide
float pulse(float t, float center, float width) {
  float x = (t - center) / width;
  return exp(-x * x);
}

// FINE: regular 60 bpm beat: small bump, dip, tall spike, dip, rounded wave
float fine_ecg(unsigned long now) {
  float t = now % 1000;
  return 0.12 * pulse(t, 150, 35)
       - 0.15 * pulse(t, 265, 10)
       + 1.00 * pulse(t, 285, 12)
       - 0.35 * pulse(t, 305, 12)
       + 0.30 * pulse(t, 520, 60);
}

// CAUTION: uneven spikes of different heights over a 2.4 s pattern, one big one
const float CAUTION_AT[7] = {150, 480, 780, 1050, 1350, 1900, 2150};
const float CAUTION_H[7]  = {0.40, 0.50, 0.45, 1.00, 0.30, 0.70, 0.35};

float caution_ecg(unsigned long now) {
  float t = now % 2400;
  float v = 0.08 * sin(2 * PI * t / 400.0);
  for (int i = 0; i < 7; i++) {
    v += CAUTION_H[i] * pulse(t, CAUTION_AT[i], 14)
       - 0.3 * CAUTION_H[i] * pulse(t, CAUTION_AT[i] + 22, 12);
  }
  return v;
}

// DANGER: chaotic wobble. Each term uses its own remainder so it stays
// precise even after the board has been running for hours.
float danger_ecg(unsigned long now) {
  return 0.50 * sin(2 * PI * (now % 310) / 310.0)
       + 0.33 * sin(2 * PI * (now % 170) / 170.0 + 1.0)
       + 0.17 * sin(2 * PI * (now % 97)  / 97.0  + 2.0);
}

// turns the trace into light brightness (0..1): lights only follow the upward parts,
// except DANGER, which never settles
float led_wave(Health h, unsigned long now) {
  float v;
  if (h == FINE)         v = fine_ecg(now);
  else if (h == CAUTION) v = caution_ecg(now);
  else                   return 0.5 + 0.5 * danger_ecg(now);
  return v < 0.0 ? 0.0 : v;
}

bool pad_pressed(int pad) {
  return (touch_state >> pad) & 1;
}

// the chosen state stays until another pad is pressed
void update_health_from_pads() {
  if (pad_pressed(PAD_UP))    health = OFF;
  if (pad_pressed(PAD_LEFT))  health = CAUTION;
  if (pad_pressed(PAD_DOWN))  health = DANGER;
  if (pad_pressed(PAD_RIGHT)) health = FINE;

  if (pad_pressed(PAD_WAVES)) view = WAVES;
  if (pad_pressed(PAD_LOGO))  view = LOGO;
}

// any touch wakes the screen; that first touch is only a wake-up,
// so it doesn't change the health state or view
void update_screensaver() {
  unsigned long now = millis();

  if (touch_state != 0) {
    last_touch_ms = now;
    if (screen_asleep) {
      screen_asleep = false;
      wake_touch_held = true;
      display.ssd1306_command(SSD1306_DISPLAYON);
    }
  } else {
    wake_touch_held = false;
  }

  if (!screen_asleep && now - last_touch_ms > SCREENSAVER_MS) {
    screen_asleep = true;
    display.ssd1306_command(SSD1306_DISPLAYOFF);
  }
}

// after each frame, tell the laptop what to draw next:
// 1 status byte (bits 0-1: health, bit 2: view) + 4 bytes of this board's clock,
// so the laptop draws the trace on the same timeline the lights use
void send_status() {
  uint8_t status = (uint8_t)health | ((uint8_t)view << 2);
  uint32_t now = millis();
  Serial.write(status);
  Serial.write((uint8_t*)&now, 4);
}

// non-blocking, driven off millis() so it never stalls
// the serial/display loop the way a delay()-based version would
void update_led() {
  unsigned long now = millis();

  if (AUTO_CYCLE) {
    health = (Health)((now / STATE_DURATION_MS) % 3);
  }

  if (health == OFF) {
    setPixel(0, 0, 0, 0.0);
  } else if (health == FINE) {
    setPixel(0, 255, 0, led_wave(FINE, now));         // green
  } else if (health == CAUTION) {
    setPixel(255, 176, 0, led_wave(CAUTION, now));    // site amber #ffb000
  } else {
    setPixel(255, 0, 0, led_wave(DANGER, now));       // red
  }
}

void setup() {
  Serial.begin(0);

  pinMode(PIN_RED, OUTPUT);
  pinMode(PIN_GREEN, OUTPUT);
  pinMode(PIN_BLUE, OUTPUT);
  digitalWrite(PIN_RED, HIGH);
  digitalWrite(PIN_GREEN, HIGH);
  digitalWrite(PIN_BLUE, HIGH);

  pinMode(NEOPIXEL_POWER, OUTPUT);
  digitalWrite(NEOPIXEL_POWER, HIGH);
  pixel.begin();
  setPixel(0, 0, 0, 0.0);

  // give the screen some time to power up
  delay(50);

  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.print("Waiting for\nconnection...");
  display.display();

  buffer_pointer = display.getBuffer();
}

size_t decode_hdlc(char* buf_in, size_t length_in, char* buf_out, size_t length_out) {
  int i, j;
  char c_in, c_out;

  i = 0;
  j = 0;

  while (i < length_in && j < length_out) {
    c_in = buf_in[i];

    if (c_in == HDLC_ESC) {
      i++;
      c_in = buf_in[i];
      if (c_in == HDLC_FLAG_ENC) {
        c_out = HDLC_FLAG;
      } else {
        c_out = HDLC_ESC;
      }
    } else {
      c_out = c_in;
    }

    buf_out[j] = c_out;

    i++;
    j++;
  }
  return j;
}

void loop() {
  size_t n_read;
  size_t n_decoded;
  char c;

  update_touch();
  update_screensaver();
  if (!wake_touch_held) {
    update_health_from_pads();
  }
  update_led();

  if (Serial.available()) {
    c = Serial.read();
    if (c == HDLC_FLAG) {
      // found a delimiter, read the rest
      n_read = Serial.readBytesUntil(HDLC_FLAG, buffer_in, LENGTH_BUFFER_INPUT);

      if (n_read == 0) {
        // immediately found another delimiter, try to read the rest
        n_read = Serial.readBytesUntil(HDLC_FLAG, buffer_in, LENGTH_BUFFER_INPUT);
      }

      // decode the HDLC encoding
      n_decoded = decode_hdlc(buffer_in, n_read, (char*)buffer_pointer, LENGTH_BUFFER);

      // succesfully recovered 1024 bytes, display it
      if (n_decoded == LENGTH_BUFFER) {
        display.display();
        send_status();
      }
    }
  }
}