#include <Arduino.h>
#include <Adafruit_TiCoServo.h>
#include <NeoPixelBus.h>

// -------------------- PIN DEFINES --------------------
#define CENTER_PIXEL_PIN 4
#define RING_PIXEL_PIN   3

#define SERVO_YAW_PIN    9
#define SERVO_PITCH_PIN 10

#define BUTTON1_PIN      11
#define BUTTON2_PIN      12
#define BUTTON3_PIN      8
#define BUTTON4_PIN      7

// -------------------- OTHER DEFINES --------------------
#define NUM_BUTTONS 4
#define NUM_PIXELS 16

#define BRIGHTNESS_MIN  13
#define BRIGHTNESS_MED  51
#define BRIGHTNESS_MAX 255

#define YAW_HOME   100
#define PITCH_HOME 120

// -------------------- GLOBALS --------------------
Adafruit_TiCoServo servoYaw;
Adafruit_TiCoServo servoPitch;

NeoPixelBus<NeoGrbFeature, Neo800KbpsMethod> ring(NUM_PIXELS, RING_PIXEL_PIN);
NeoPixelBus<NeoGrbFeature, Neo800KbpsMethod> center(1, CENTER_PIXEL_PIN);

int pinputs[NUM_BUTTONS]        = {BUTTON1_PIN, BUTTON2_PIN, BUTTON3_PIN, BUTTON4_PIN};
int buttonStates[NUM_BUTTONS]   = {0, 0, 0, 0};
int lastButtonStates[NUM_BUTTONS] = {0, 0, 0, 0};
unsigned long lastDebounceTime[NUM_BUTTONS] = {0, 0, 0, 0};

const unsigned long debounceDelay = 50;

int movementMode   = 0;
int colorMode      = 0;
int brightnessMode = 0;
int speedMode      = 0;

int yawTarget   = YAW_HOME;
int pitchTarget = PITCH_HOME;

float yawCurrent   = YAW_HOME;    // float for smooth movement
float pitchCurrent = PITCH_HOME;

unsigned long lastTargetTime = 0;

// -------------------- PROTOTYPES --------------------
void handleButtonPress(int btn);
void updateMovement(unsigned long now);
void updateColor(unsigned long now);
RgbColor scaleBrightness(RgbColor c, uint8_t b);

// -------------------- SETUP --------------------
void setup() {
  Serial.begin(115200);

  for (int i = 0; i < NUM_BUTTONS; i++) {
    pinMode(pinputs[i], INPUT_PULLUP);
    buttonStates[i] = digitalRead(pinputs[i]);
    lastButtonStates[i] = buttonStates[i];
  }

  servoYaw.attach(SERVO_YAW_PIN);
  servoPitch.attach(SERVO_PITCH_PIN);
  servoYaw.write(YAW_HOME);
  servoPitch.write(PITCH_HOME);

  ring.Begin();
  ring.Show();
  center.Begin();
  center.Show();
}

// -------------------- LOOP --------------------
void loop() {
  unsigned long now = millis();

  // Button reading + debounce
  for (int i = 0; i < NUM_BUTTONS; i++) {
    int reading = digitalRead(pinputs[i]);
    if (reading != lastButtonStates[i]) lastDebounceTime[i] = now;

    if ((now - lastDebounceTime[i]) > debounceDelay) {
      if (reading != buttonStates[i]) {
        buttonStates[i] = reading;
        if (buttonStates[i] == LOW) handleButtonPress(i);
      }
    }
    lastButtonStates[i] = reading;
  }

  updateMovement(now);
  updateColor(now);
}

// -------------------- BUTTON HANDLER --------------------
void handleButtonPress(int btn) {
  switch (btn) {
    case 0: movementMode   = (movementMode   + 1) % 4; break;
    case 1: colorMode      = (colorMode      + 1) % 4; break;
    case 2: brightnessMode = (brightnessMode + 1) % 4; break;
    case 3: speedMode      = (speedMode      + 1) % 4; break;
  }
  Serial.print("Modes M:");
  Serial.print(movementMode);
  Serial.print(" C:");
  Serial.print(colorMode);
  Serial.print(" B:");
  Serial.print(brightnessMode);
  Serial.print(" S:");
  Serial.println(speedMode);
}

// -------------------- MOVEMENT (Slow + Smooth) --------------------
void updateMovement(unsigned long now) {

  // Speed control
  float ease;
  unsigned long targetInterval;

  switch (speedMode) {
    case 0: ease = 0.008; targetInterval = 3000; break;
    case 1: ease = 0.015; targetInterval = 2000; break;
    case 2: ease = 0.03;  targetInterval = 1200; break;
    case 3: ease = 0.06;  targetInterval = 600;  break;
  }

  switch (movementMode) {

    // ------------------ 0: HOME ------------------
    case 0:
      yawTarget   = 90;
      pitchTarget = 90;
      yawCurrent   += (yawTarget   - yawCurrent)   * ease;
      pitchCurrent += (pitchTarget - pitchCurrent) * ease;
      break;

    // ------------------ 1: RANDOM JUMP ------------------
    case 1:
      if (now - lastTargetTime > targetInterval) {
        lastTargetTime = now;
        yawCurrent   = random(30, 150);
        pitchCurrent = random(30, 150);
      }
      break;

    // ------------------ 2: SMOOTH RANDOM ------------------
    case 2:
      if (now - lastTargetTime > targetInterval) {
        lastTargetTime = now;
        yawTarget   = random(30, 150);
        pitchTarget = random(30, 150);
      }
      yawCurrent   += (yawTarget   - yawCurrent)   * ease;
      pitchCurrent += (pitchTarget - pitchCurrent) * ease;
      break;

    // ------------------ 3: SIN-BASED SMOOTH ------------------
    case 3: {
      float baseSpeed = 0.00025 + speedMode * 0.00015;
      yawCurrent   = 90 + sin(now * baseSpeed) * 60;
      pitchCurrent = 90 + sin(now * baseSpeed * 0.73 + 1.8) * 35;
      break;
    }
  }

  servoYaw.write((int)yawCurrent);
  servoPitch.write((int)pitchCurrent);
}

// -------------------- COLOR + BRIGHTNESS --------------------
void updateColor(unsigned long now) {
  uint8_t brightness = (brightnessMode == 0 ? BRIGHTNESS_MIN :
                        brightnessMode == 1 ? BRIGHTNESS_MED :
                        brightnessMode == 2 ? BRIGHTNESS_MAX : 0);

  switch (colorMode) {
    case 0:
      for (int i=0;i<NUM_PIXELS;i++)
        ring.SetPixelColor(i, scaleBrightness(RgbColor(255,255,255), brightness));
      center.SetPixelColor(0, scaleBrightness(RgbColor(255,255,255), brightness));
      break;

    case 1: {
      int r = (sin(now/500.0)*127)+128;
      int g = (sin(now/600.0)*127)+128;
      int b = (sin(now/700.0)*127)+128;
      RgbColor c(r,g,b);
      for (int i=0;i<NUM_PIXELS;i++)
        ring.SetPixelColor(i, scaleBrightness(c, brightness));
      center.SetPixelColor(0, scaleBrightness(c, brightness));
      break;
    }

    case 2:
      for (int i=0;i<NUM_PIXELS;i++) {
        int r = (i*16 + now/10) % 256;
        int g = (i*32 + now/15) % 256;
        int b = (i*64 + now/20) % 256;
        ring.SetPixelColor(i, scaleBrightness(RgbColor(r,g,b), brightness));
      }
      center.SetPixelColor(0, scaleBrightness(RgbColor(255,255,255), brightness));
      break;

    case 3:
      for (int i=0;i<NUM_PIXELS;i++)
        ring.SetPixelColor(i, scaleBrightness(RgbColor(random(0,256), random(0,256), random(0,256)), brightness));
      int cval = random(0,2)?255:0;
      center.SetPixelColor(0, scaleBrightness(RgbColor(cval,cval,cval), brightness));
      break;
  }

  ring.Show();
  center.Show();
}

// -------------------- BRIGHTNESS SCALE --------------------
RgbColor scaleBrightness(RgbColor c, uint8_t b) {
  return RgbColor(((uint16_t)c.R * b) / 255,
                  ((uint16_t)c.G * b) / 255,
                  ((uint16_t)c.B * b) / 255);
}