/*
  NSTRV Mini Moving Head
  Firmware: head v0.3
  Controller: ESP32-C3
  Motors: 2x SG90
  LED: RGB, MOSFET control
  Input: touch sensor, active LOW

  Controls:
  Single tap  - ON / STANDBY
  Double tap  - AUTO / SOUND
  Triple tap  - AUTO SMOOTH / AUTO ACTIVE
  Hold        - brightness up/down, alternating per hold
*/

#include <ESP32Servo.h>

// ---------- PINS ----------

const int PIN_PAN   = 4;
const int PIN_TILT  = 3;

const int PIN_RED   = 6;
const int PIN_GREEN = 7;
const int PIN_BLUE  = 8;

const int PIN_TOUCH = 10;

// ---------- SERVO LIMITS ----------

const int PAN_MIN  = 15;
const int PAN_MAX  = 165;

const int TILT_MIN = 25;
const int TILT_MAX = 150;

const int PAN_HOME  = 90;
const int TILT_HOME = 30;

// If TILT_HOME points upward instead of downward,
// change it to 150 after checking the mechanism.

// ---------- INPUT SETTINGS ----------

const unsigned long DEBOUNCE_MS = 35;
const unsigned long CLICK_WINDOW_MS = 350;
const unsigned long HOLD_START_MS = 500;
const unsigned long BRIGHTNESS_STEP_MS = 45;

// ---------- SERVO SPEED ----------

//const unsigned long SERVO_STEP_MS = 1000;
const unsigned long SERVO_STEP_MS = 200;

// One degree per step.
// Increase SERVO_STEP_MS for slower movement.

const unsigned long COLOR_STEP_MS = 10;

// ---------- BRIGHTNESS ----------

const int BRIGHTNESS_MIN = 5;
const int BRIGHTNESS_MAX = 100;

int brightness = 100;
int brightnessDirection = 1;

// First hold increases brightness.
// Each subsequent hold reverses direction.

// ---------- OBJECTS ----------

Servo servoPan;
Servo servoTilt;

// ---------- MODES ----------

enum MainMode {
  MODE_AUTO,
  MODE_SOUND
};

enum AutoStyle {
  AUTO_SMOOTH,
  AUTO_ACTIVE
};

MainMode mainMode = MODE_AUTO;
AutoStyle autoStyle = AUTO_SMOOTH;

bool isOn = true;

// ---------- SERVO STATE ----------

int panPosition = PAN_HOME;
int tiltPosition = TILT_HOME;

int panTarget = PAN_HOME;
int tiltTarget = TILT_HOME;

unsigned long lastServoStep = 0;
unsigned long nextMoveTime = 0;

// ---------- RGB STATE ----------

int redLevel = 255;
int greenLevel = 255;
int blueLevel = 255;

int redTarget = 255;
int greenTarget = 0;
int blueTarget = 0;

unsigned long lastColorStep = 0;
unsigned long nextColorTime = 0;

// ---------- TOUCH STATE ----------

bool lastRawTouch = HIGH;
bool stableTouch = HIGH;

unsigned long lastDebounceTime = 0;
unsigned long pressStartTime = 0;
unsigned long lastBrightnessStep = 0;
unsigned long lastReleaseTime = 0;

bool holdProcessed = false;
bool holdDirectionPending = false;

int clickCount = 0;

// ---------- RANDOM HELPERS ----------

int randomBetween(int low, int high) {
  return random(low, high + 1);
}

// ---------- LED CONTROL ----------

void setupLED() {
  // ESP32 Arduino Core 3.x API
  ledcAttach(PIN_RED, 5000, 8);
  ledcAttach(PIN_GREEN, 5000, 8);
  ledcAttach(PIN_BLUE, 5000, 8);
}

void updateLED() {
  if (!isOn) {
    ledcWrite(PIN_RED, 0);
    ledcWrite(PIN_GREEN, 0);
    ledcWrite(PIN_BLUE, 0);
    return;
  }

  int master = map(
    brightness,
    0, 100,
    0, 255
  );

  int r = (redLevel * master) / 255;
  int g = (greenLevel * master) / 255;
  int b = (blueLevel * master) / 255;

  ledcWrite(PIN_RED, r);
  ledcWrite(PIN_GREEN, g);
  ledcWrite(PIN_BLUE, b);
}

void chooseNewColor() {
  // Select one of six saturated RGB colors.
  int color = random(0, 6);

  switch (color) {
    case 0:
      redTarget = 255;
      greenTarget = 0;
      blueTarget = 0;
      break;

    case 1:
      redTarget = 255;
      greenTarget = 100;
      blueTarget = 0;
      break;

    case 2:
      redTarget = 0;
      greenTarget = 255;
      blueTarget = 0;
      break;

    case 3:
      redTarget = 0;
      greenTarget = 255;
      blueTarget = 255;
      break;

    case 4:
      redTarget = 0;
      greenTarget = 0;
      blueTarget = 255;
      break;

    case 5:
      redTarget = 255;
      greenTarget = 0;
      blueTarget = 255;
      break;
  }
}

void updateColor() {
  if (!isOn) return;

  unsigned long now = millis();

  // Choose a new color periodically in AUTO.
  if (mainMode == MODE_AUTO && now >= nextColorTime) {
    chooseNewColor();

    if (autoStyle == AUTO_SMOOTH) {
      nextColorTime = now + randomBetween(3000, 6000);
    } else {
      nextColorTime = now + randomBetween(500, 1600);
    }
  }

  if (now - lastColorStep < COLOR_STEP_MS) return;
  lastColorStep = now;

  // Smooth RGB transitions.
  const int colorStep = 3;

  if (redLevel < redTarget) {
    redLevel = min(redLevel + colorStep, redTarget);
  } else if (redLevel > redTarget) {
    redLevel = max(redLevel - colorStep, redTarget);
  }

  if (greenLevel < greenTarget) {
    greenLevel = min(greenLevel + colorStep, greenTarget);
  } else if (greenLevel > greenTarget) {
    greenLevel = max(greenLevel - colorStep, greenTarget);
  }

  if (blueLevel < blueTarget) {
    blueLevel = min(blueLevel + colorStep, blueTarget);
  } else if (blueLevel > blueTarget) {
    blueLevel = max(blueLevel - colorStep, blueTarget);
  }

  updateLED();
}

// ---------- SERVO CONTROL ----------

void setHomePosition() {
  panTarget = PAN_HOME;
  tiltTarget = TILT_HOME;
}

void chooseMovementTarget() {
  if (!isOn || mainMode == MODE_SOUND) return;

  if (autoStyle == AUTO_SMOOTH) {
    panTarget = randomBetween(PAN_MIN, PAN_MAX);
    tiltTarget = randomBetween(TILT_MIN, TILT_MAX);
  } else {
    panTarget = randomBetween(PAN_MIN, PAN_MAX);
    tiltTarget = randomBetween(TILT_MIN, TILT_MAX);
  }

  nextMoveTime = millis() + 20;
}

void updateServos() {
  if (!isOn || mainMode == MODE_SOUND) {
    // В режиме ожидания движение к домашней позиции
    if (!isOn) {
      panTarget = PAN_HOME;
      tiltTarget = TILT_HOME;
    } else {
      return;
    }
  }

  unsigned long now = millis();

  if (now - lastServoStep < SERVO_STEP_MS) return;
  lastServoStep = now;

  // Плавное движение PAN
  if (panPosition < panTarget) {
    panPosition++;
    servoPan.write(panPosition);
  } else if (panPosition > panTarget) {
    panPosition--;
    servoPan.write(panPosition);
  }

  // Плавное движение TILT
  if (tiltPosition < tiltTarget) {
    tiltPosition++;
    servoTilt.write(tiltPosition);
  } else if (tiltPosition > tiltTarget) {
    tiltPosition--;
    servoTilt.write(tiltPosition);
  }

  // Если оба сервопривода достигли целей,
  // сразу назначаем новые позиции
  if (panPosition == panTarget &&
      tiltPosition == tiltTarget &&
      isOn &&
      mainMode == MODE_AUTO) {
    chooseMovementTarget();
  }

  Serial.print(panPosition), Serial.print(" P_TAG= "), Serial.print(panTarget);
  Serial.print("       ");
  Serial.print(tiltPosition), Serial.print(" T_TAG= "), Serial.println(tiltTarget); 
}

// ---------- STANDBY ----------

void enterStandby() {
  isOn = false;

  setHomePosition();

  redTarget = 0;
  greenTarget = 0;
  blueTarget = 0;

  updateLED();
}

void wakeUp() {
  isOn = true;

  mainMode = MODE_AUTO;
  autoStyle = AUTO_SMOOTH;

  setHomePosition();

  chooseNewColor();

  nextMoveTime = millis() + 2000;
  nextColorTime = millis() + 1000;
}

// ---------- BRIGHTNESS ----------

void startBrightnessHold() {
  brightnessDirection = -brightnessDirection;
  holdDirectionPending = false;

  // A pending direction change is handled when
  // a hold is confirmed, not on an ordinary tap.
}

void changeBrightness() {
  brightness += brightnessDirection;

  brightness = constrain(
    brightness,
    BRIGHTNESS_MIN,
    BRIGHTNESS_MAX
  );

  updateLED();
}

// ---------- CLICK ACTIONS ----------

void processClicks() {
  if (clickCount == 0) return;

  if (clickCount == 1) {
    if (isOn) {
      enterStandby();
    } else {
      wakeUp();
    }
  }

  else if (clickCount == 2) {
    if (isOn) {
      if (mainMode == MODE_AUTO) {
        mainMode = MODE_SOUND;
      } else {
        mainMode = MODE_AUTO;
        nextMoveTime = millis();
      }
    }
  }

  else if (clickCount >= 3) {
    if (isOn && mainMode == MODE_AUTO) {
      if (autoStyle == AUTO_SMOOTH) {
        autoStyle = AUTO_ACTIVE;
      } else {
        autoStyle = AUTO_SMOOTH;
      }

      nextMoveTime = millis();
      nextColorTime = millis();
    }
  }

  clickCount = 0;
}

// ---------- TOUCH INPUT ----------

void updateTouch() {
  unsigned long now = millis();
  bool rawTouch = digitalRead(PIN_TOUCH);

  if (rawTouch != lastRawTouch) {
    lastDebounceTime = now;
    lastRawTouch = rawTouch;
  }

  if (now - lastDebounceTime < DEBOUNCE_MS) return;

  if (rawTouch != stableTouch) {
    stableTouch = rawTouch;

    if (stableTouch == LOW) {
      // Touch/press started.
      pressStartTime = now;
      holdProcessed = false;
      lastBrightnessStep = now;
    } else {
      // Touch/press released.
      if (!holdProcessed) {
        clickCount++;
        lastReleaseTime = now;
      }
    }
  }

  // Long press: smoothly change brightness.
  if (stableTouch == LOW &&
      !holdProcessed &&
      now - pressStartTime >= HOLD_START_MS) {

    holdProcessed = true;

    // Cancel taps accumulated before this hold.
    clickCount = 0;

    // Alternate direction on each confirmed hold.
    brightnessDirection = -brightnessDirection;

    lastBrightnessStep = now;
  }

  if (stableTouch == LOW && holdProcessed) {
    if (now - lastBrightnessStep >= BRIGHTNESS_STEP_MS) {
      lastBrightnessStep = now;
      changeBrightness();
    }
  }

  // Wait for the click sequence to finish.
  if (stableTouch == HIGH &&
      clickCount > 0 &&
      now - lastReleaseTime >= CLICK_WINDOW_MS) {
    processClicks();
  }
}

// ---------- SETUP ----------

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

  randomSeed(micros());

  pinMode(PIN_TOUCH, INPUT_PULLUP);

 // setupLED();

  //servoPan.setPeriodHertz(50);
  //servoTilt.setPeriodHertz(50);

  servoPan.attach(PIN_PAN, 500, 2400);
  servoTilt.attach(PIN_TILT, 500, 2400);

  // Start in the center/home position.
  servoPan.write(PAN_HOME);
  servoTilt.write(TILT_HOME);

  panPosition = PAN_HOME;
  tiltPosition = TILT_HOME;

  Serial.println("Servo_Start");
  delay(1000);

   setupLED();

  isOn = true;
  mainMode = MODE_AUTO;
  autoStyle = AUTO_SMOOTH;

  chooseNewColor();

  nextMoveTime = millis() + 2000;
  nextColorTime = millis() + 1000;

  updateLED();

  Serial.println("NSTRV Mini Moving Head");
  Serial.println("Firmware: head v0.3");
  Serial.println("Mode: AUTO SMOOTH");
}

// ---------- MAIN LOOP ----------

void loop() {
  updateTouch();
  updateServos();
  updateColor();
}