const int PIN_SERVO     = 22;
const int PIN_POT       = 34;
const int PIN_LED_GREEN = 12;
const int PIN_LED_RED   = 13;

const float DEVIATION   = 10.0;   
const float SPEED       = 15.0;   
const int   PAUSE_MS    = 400; 
const int   SERVO_FREQ  = 50; 
const int   SERVO_RES   = 11; 


float currentAngle = 90.0;
float targetAngle  = 90.0;
float centerAngle  = 90.0;

int   step         = 0; 
bool  isPausing    = false;
unsigned long pauseStart  = 0;
unsigned long lastMove    = 0;
unsigned long lastPrint   = 0;
bool  protection   = false;


void setServoPWM(float angle) {
  float a = constrain(angle, 0.0, 180.0);
  int pulse_us = map(a, 0, 180, 500, 2500);
  int duty = (pulse_us * ((1 << SERVO_RES) - 1)) / 20000;
  
  ledcWrite(PIN_SERVO, duty);
}

void setup() {
  Serial.begin(115200);
  analogReadResolution(12);
  pinMode(PIN_LED_GREEN, OUTPUT);
  pinMode(PIN_LED_RED, OUTPUT);
  ledcAttach(PIN_SERVO, SERVO_FREQ, SERVO_RES);
  setServoPWM(90);
  currentAngle = 90.0;
  targetAngle  = 90.0;
  lastMove     = millis();
}

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

  int potVal = analogRead(PIN_POT);
  centerAngle = potVal * 180.0 / 4095.0;

  float offset = 0;
  if (step == 1)      offset =  DEVIATION;
  else if (step == 3) offset = -DEVIATION;
  
  float rawTarget = centerAngle + offset;
  
  protection = (rawTarget < 0.0 || rawTarget > 180.0);
  targetAngle = constrain(rawTarget, 0.0, 180.0);
  
  digitalWrite(PIN_LED_GREEN, protection ? LOW : HIGH);
  digitalWrite(PIN_LED_RED,   protection ? HIGH : LOW);
  
  if (!isPausing) {
    float dt = (now - lastMove) / 1000.0;
    lastMove = now;
    
    float moveStep = SPEED * dt;
    
    if (currentAngle < targetAngle) {
      currentAngle += moveStep;
      if (currentAngle >= targetAngle) {
        currentAngle = targetAngle;
        isPausing = true;
        pauseStart = now;
      }
    } 
    else if (currentAngle > targetAngle) {
      currentAngle -= moveStep;
      if (currentAngle <= targetAngle) {
        currentAngle = targetAngle;
        isPausing = true;
        pauseStart = now;
      }
    }
    setServoPWM(currentAngle);
  } 
  else {
    // Режим паузы
    if (now - pauseStart >= PAUSE_MS) {
      isPausing = false;
      step = (step + 1) % 4;
      lastMove = now;
    }
  }

  if (now - lastPrint >= 200) {
    lastPrint = now;
    
    float t2 = constrain(centerAngle + DEVIATION, 0, 180);
    float t4 = constrain(centerAngle - DEVIATION, 0, 180);
    float dist = 2.0 * abs(t2 - centerAngle) + 2.0 * abs(t4 - centerAngle);
    
    float moveTime  = (dist / SPEED) * 1000.0;
    float cycleTime = moveTime + 4.0 * PAUSE_MS;
    
    Serial.print("Центр: "); Serial.print(centerAngle, 1); Serial.print("° | ");
    Serial.print("Цель: ");  Serial.print(targetAngle, 1);  Serial.print("° | ");
    Serial.print("Текущий угол: "); Serial.print(currentAngle, 1); Serial.print("° | ");
    Serial.print("Время полного цикла: "); Serial.print((int)cycleTime); Serial.print(" мс | ");
    
    if (protection) {
      Serial.println("R+: сработала защита: ОГРАНИЧЕНИЕ УГЛА");
    } else {
      Serial.println("G+: В НОРМЕ");
    }
  }
}