/*
Guitar_adapter_winding_machine
Автоматичен брояч на намотки с дебаунс функция
*/

#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <Encoder.h>
#include <Servo.h>

// OLED Display
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// Ротационен енкодер
#define ENCODER_CLK 2
#define ENCODER_DT 3
#define ENCODER_BTN 4
Encoder encoder(ENCODER_CLK, ENCODER_DT);

// Моторно управление
#define MOTOR_PWM 9
#define MOTOR_DIR 8
#define MOTOR_BRAKE 7

// Серво мотор
#define SERVO_PIN 6
Servo myServo;

// Оптичен сензор
#define OPTICAL_SENSOR 5
volatile bool sensorTriggered = false;
volatile bool lastOpticalState = HIGH;
unsigned long lastDebounceTime = 0;
#define DEBOUNCE_DELAY 10  // Дебаунс време в мс

// Глобални променливи
volatile int currentWindings = 0;
int setWindings = 20;
int maxSpeed = 100;
bool motorRunning = false;
bool motorDirection = false;
int menuState = 0;
int targetServoAngle = 10;
int servoAngle = 0;
bool servoMovingForward = true;

// Прекъсване за оптичен сензор
ISR(PCINT2_vect) {
    bool currentState = digitalRead(OPTICAL_SENSOR);
    if (currentState == LOW && lastOpticalState == HIGH) {
        sensorTriggered = true;
    }
    lastOpticalState = currentState;
}

void startMotor() {
    motorRunning = true;
    currentWindings = 0;
    digitalWrite(MOTOR_BRAKE, HIGH);
    digitalWrite(MOTOR_DIR, motorDirection);
    
    for (int i = 0; i <= maxSpeed; i += 1) {
        analogWrite(MOTOR_PWM, map(i, 0, 100, 0, 255));
        delay(30);
    }
}

void stopMotor() {
    for (int i = maxSpeed; i >= 0; i -= 10) {
        analogWrite(MOTOR_PWM, map(i, 100, 0, 255, 0));
        delay(30);
    }
    digitalWrite(MOTOR_BRAKE, LOW);
    motorRunning = false;
}

void changeDirection() {
    if (motorRunning) {
        stopMotor();
        delay(300);
        motorDirection = !motorDirection;
        startMotor();
    } else {
        motorDirection = !motorDirection;
    }
}

void moveServo() {
    if (motorRunning) {
        if (servoMovingForward) {
            servoAngle = min(servoAngle + 1, targetServoAngle);
            if (servoAngle >= targetServoAngle) servoMovingForward = false;
        } else {
            servoAngle = max(servoAngle - 1, 0);
            if (servoAngle <= 0) servoMovingForward = true;
        }
        myServo.write(servoAngle);
    } else {
        myServo.write(0);
    }
}

void setup() {
    Serial.begin(115200);
    pinMode(MOTOR_PWM, OUTPUT);
    pinMode(MOTOR_DIR, OUTPUT);
    pinMode(MOTOR_BRAKE, OUTPUT);
    pinMode(ENCODER_BTN, INPUT_PULLUP);
    pinMode(OPTICAL_SENSOR, INPUT_PULLUP);

    // Настройка на прекъсванията
    PCICR |= (1 << PCIE2);
    PCMSK2 |= (1 << PCINT5);
    
    // Инициализация на серво
    myServo.attach(SERVO_PIN);
    myServo.write(0);

    // Инициализация на дисплей
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
        while(1);
    }
    display.clearDisplay();
    encoder.write(0);
}

void loop() {
    static long lastPosition = 0;
    long newPosition = encoder.read() / 4;

    // Дебаунс логика
    if (sensorTriggered && (millis() - lastDebounceTime) > DEBOUNCE_DELAY) {
        noInterrupts();
        bool verifiedState = digitalRead(OPTICAL_SENSOR);
        interrupts();

        if (verifiedState == LOW && motorRunning) {
            currentWindings++;
            if (currentWindings >= setWindings) stopMotor();
        }
        sensorTriggered = false;
        lastDebounceTime = millis();
    }

    // Обработка на енкодера
    if (newPosition != lastPosition) {
        int delta = lastPosition - newPosition;
        
        switch(menuState) {
            case 0: 
                setWindings = constrain(setWindings + delta, 0, 99999);
                break;
            case 1:
                maxSpeed = constrain(maxSpeed + delta, 0, 100);
                break;
            case 2:
                changeDirection();
                break;
            case 3:
                targetServoAngle = constrain(targetServoAngle + delta, 0, 180);
                break;
        }
        lastPosition = newPosition;
    }

    // Бутон на енкодера
    if (digitalRead(ENCODER_BTN) == LOW) {
        delay(200);
        menuState = (menuState + 1) % 5;
        encoder.write(0);
        lastPosition = 0;
        
        if (menuState == 4) {
            motorRunning ? stopMotor() : startMotor();
        }
    }

    // Управление на сервото
    moveServo();

    // Update OLED Display
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(WHITE);
    display.setCursor(0, 0);
    display.print(menuState == 0 ? ">Set Windings: " : " Set Windings: "); display.println(setWindings);
    display.print(menuState == 1 ? ">Set Speed: " : " Set Speed: "); display.print(maxSpeed); display.println("%");
    display.print(menuState == 2 ? ">Direction: " : " Direction: "); display.println(motorDirection ? "Reverse" : "Forward");
    display.print(menuState == 3 ? ">Servo Angle: " : " Servo Angle: "); display.println(targetServoAngle); // Показване на настроения ъгъл на сервото
    display.print(menuState == 4 ? ">Motor: " : " Motor: "); display.println(motorRunning ? "Running" : "Stopped");
    display.println("         ");
    display.print(" Current Wind.: "); display.println(currentWindings); // Показване на текущия брой намотки
    
    display.display();
}