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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Pin mapping (from your diagram.json)
const int PIN_HR   = 34;   // Pot1: Heart rate proxy
const int PIN_MOVE = 35;   // Pot2: Movement proxy
const int PIN_BTN  = 25;   // Pushbutton
const int PIN_BUZZ = 12;   // Buzzer
const int PIN_LED  = 4;    // NeoPixel Ring

// NeoPixel setup
#define LED_COUNT 16
Adafruit_NeoPixel pixels(LED_COUNT, PIN_LED, NEO_GRB + NEO_KHZ800);

// States
enum State { NORMAL, PRE_ALERT, CALMING };
State state = NORMAL;

// Smoothed values
float hrAvg = 80, mvAvg = 0;
const float alpha = 0.12;

// Thresholds
const float HR_ALERT = 110.0;  
const float MV_ALERT = 70.0;   

// Timers
unsigned long preAlertStart = 0;
const unsigned long PRE_ALERT_HOLD = 2000; // 2 sec
unsigned long calmingStart = 0;
const unsigned long CALMING_MIN = 6000;   // 6 sec

// Helper: map float
float mapf(long x,long in_min,long in_max,float out_min,float out_max) {
  return (float)(x - in_min) * (out_max - out_min) / (float)(in_max - in_min) + out_min;
}

// NeoPixel effects
void neoClear() {
  for(int i=0;i<LED_COUNT;i++) pixels.setPixelColor(i,0);
  pixels.show();
}

void neoBreathe(uint8_t r, uint8_t g, uint8_t b, unsigned long t) {
  float phase = (sin(t*2*PI/3000.0) + 1.0)/2.0; // 3s cycle
  uint8_t br = 10 + (uint8_t)(phase * 80);
  pixels.setBrightness(br);
  for(int i=0;i<LED_COUNT;i++) pixels.setPixelColor(i, pixels.Color(r,g,b));
  pixels.show();
}

// Calming sound pattern (tone/noTone version)
void playCalmSound(unsigned long now) {
  float phase = fmod(now/1000.0, 5.0);
  if(phase < 2.0) {
    float f = 200 + phase*50;   // Sweep 200–300Hz
    tone(PIN_BUZZ, (int)f);
  } else {
    noTone(PIN_BUZZ);
  }
}

// OLED UI
void drawUI(float bpm, float mv, const char* s) {
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0,0);
  oled.print("Sensory Overload Demo");
  oled.setCursor(0,16); oled.print("Sim BPM: "); oled.print((int)bpm);
  oled.setCursor(0,28); oled.print("Sim Move: "); oled.print((int)mv);
  oled.setCursor(0,44); oled.print("State: "); oled.print(s);
  oled.display();
}

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

  // OLED init
  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 allocation failed");
    for(;;);
  }

  // Button
  pinMode(PIN_BTN, INPUT_PULLUP);

  // NeoPixel
  pixels.begin();
  neoClear();

  // Buzzer
  pinMode(PIN_BUZZ, OUTPUT);
  noTone(PIN_BUZZ);

  drawUI(0,0,"BOOT");
}

void loop() {
  unsigned long now = millis();

  // Simulated inputs from pots
  float bpm = mapf(analogRead(PIN_HR), 0, 4095, 60, 140);
  float mv  = mapf(analogRead(PIN_MOVE), 0, 4095, 0, 100);

  // Exponential smoothing
  hrAvg = (1-alpha)*hrAvg + alpha*bpm;
  mvAvg = (1-alpha)*mvAvg + alpha*mv;

  bool manual = (digitalRead(PIN_BTN) == LOW); // pressed = LOW

  // --- State Machine ---
  switch(state) {
    case NORMAL:
      noTone(PIN_BUZZ);
      neoBreathe(0, 30, 60, now); // calm blue
      if(hrAvg > HR_ALERT || mvAvg > MV_ALERT || manual) {
        state = PRE_ALERT;
        preAlertStart = now;
      }
      drawUI(hrAvg, mvAvg, "NORMAL");
      break;

    case PRE_ALERT:
      neoBreathe(255, 140, 0, now); // orange warning
      if((now - preAlertStart) > PRE_ALERT_HOLD || manual) {
        state = CALMING;
        calmingStart = now;
      }
      drawUI(hrAvg, mvAvg, "PRE_ALERT");
      break;

    case CALMING:
      neoBreathe(80, 10, 60, now); // lavender calming
      playCalmSound(now);
      if((now - calmingStart) > CALMING_MIN && !manual &&
         hrAvg < HR_ALERT*0.9 && mvAvg < MV_ALERT*0.9) {
        state = NORMAL;
      }
      if(manual) {
        state = NORMAL;
        noTone(PIN_BUZZ);
      }
      drawUI(hrAvg, mvAvg, "CALMING");
      break;
  }

  // --- Debug print to Serial Monitor ---
  Serial.print("BPM=");
  Serial.print(hrAvg);
  Serial.print(" | Move=");
  Serial.print(mvAvg);
  Serial.print(" | State=");
  if(state==NORMAL) Serial.println("NORMAL");
  else if(state==PRE_ALERT) Serial.println("PRE_ALERT");
  else Serial.println("CALMING");

  delay(20);
}