// Control system and UI, control Rc servo according to incoming pulses from inductive sensor
// https://forum.arduino.cc/t/control-system-and-ui-control-rc-servo-according-to-incoming-pulses-from-inductive-sensor/1416579

#include <Servo.h>

// =================== ANVÄNDARJUSTERBARA PARAMETRAR ===================

// Servot och frekvens
const int minDeflection       = 5;      // Min fysiskt servoutslag (grader)
const int maxDeflection       = 175;    // Max fysiskt servoutslag (grader)
const int BASE_ANGLE_DEFAULT  = 80;     // Basläge på servo (grader)
const int MAX_ANGLE_DEFAULT   = 150;    // Maxläge på servo (grader)
const float OFFSET_DEFAULT    = 0.0;    // Offset i grader
const float minFreqToOperate  = 2.0;    // Hz, frekvens där servo börjar röra sig
const float CURVE_DEFAULT_HZ  = 150;   // Hz vid vilket servot når MAX_ANGLE
const float minFreqTimeoutSec = 0.2;    // sekunder med låg frekvens för PARK
const bool invertServo        = false;  // true = spegelvänd servo
const int maxServoStep        = 3;      // Max graders servoändring per uppdatering
const unsigned long servoIntervalMs = 5; // ms mellan servo-steg

// Potentiometerpinnar
const int potBase   = A0; // Justering av BASE_ANGLE
const int potMax    = A1; // Justering av MAX_ANGLE
const int potCurve  = A2; // Justering av CURVE
const int potOffset = A3; // Justering av OFFSET

// Servopinnar
#define PULSE_PIN 2
#define ONOFF_PIN 3
#define SERVO_PIN 9
#define PARK 5               // PARK-läge

// =================== GLOBALA VARIABLER ===================
Servo theServo;

volatile unsigned long lastEdgeMicros = 0;
volatile float filteredFreq = 0;

int currentServoPos = PARK;
int targetServoPos = PARK;

unsigned long freqBelowMinStart = 0;
bool inPark = true;
unsigned long lastServoUpdate = 0;

// Dynamiska värden som potentiometrarna justerar
int BASE_ANGLE = BASE_ANGLE_DEFAULT;
int MAX_ANGLE  = MAX_ANGLE_DEFAULT;
float OFFSET   = OFFSET_DEFAULT;
float CURVE_INTERNAL;  // Intern skalfaktor

// =================== SETUP ===================
void setup() {
    Serial.begin(115200);
    theServo.attach(SERVO_PIN);
    theServo.write(PARK);

    pinMode(ONOFF_PIN, INPUT_PULLUP);
    pinMode(PULSE_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(PULSE_PIN), pulseISR, RISING);

    // Beräkna intern CURVE-skalning från Hz
    CURVE_INTERNAL = (CURVE_DEFAULT_HZ * CURVE_DEFAULT_HZ) / (MAX_ANGLE_DEFAULT - BASE_ANGLE_DEFAULT);
}

// =================== INTERRUPT ===================
void pulseISR() {
    unsigned long now = micros();
    unsigned long period = now - lastEdgeMicros;

    // Ignorera extremt korta pulser (<1ms)
    if (period < 1000) return;

    lastEdgeMicros = now;

    // Beräkna frekvens direkt och begränsa till max 300 Hz
    float freq = 1e6 / (float)period;
    freq = constrain(freq, 0, 300.0);

    filteredFreq = freq;
}

// =================== HJÄLPFUNKTIONER ===================
float readPotPercent(int pin) {
    float pct = ((float)analogRead(pin) - 512.0) / 512.0;
    return constrain(pct, -1.0, 1.0) * 0.25;
}

void readPots() {
    BASE_ANGLE = constrain(BASE_ANGLE_DEFAULT * (1 + readPotPercent(potBase)), minDeflection, maxDeflection-1);
    MAX_ANGLE  = constrain(MAX_ANGLE_DEFAULT  * (1 + readPotPercent(potMax)), BASE_ANGLE+1, maxDeflection);
    OFFSET     = OFFSET_DEFAULT + readPotPercent(potOffset) * (MAX_ANGLE - BASE_ANGLE);
}

// =================== SERVORÖRELSE ===================
void updateServoSmooth() {
    if (millis() - lastServoUpdate < servoIntervalMs) return;
    lastServoUpdate = millis();

    if (currentServoPos != targetServoPos) {
        currentServoPos += constrain(targetServoPos - currentServoPos, -maxServoStep, maxServoStep);
        currentServoPos = constrain(currentServoPos, minDeflection, maxDeflection);
        theServo.write(currentServoPos);
    }
}

// =================== PARK LOGIK ===================
void checkPark() {
    if (digitalRead(ONOFF_PIN) == LOW) {
        inPark = true;
        targetServoPos = currentServoPos = PARK;
        theServo.write(PARK);
        filteredFreq = 0;
        Serial.println("PARK button");
        delay(50);
        return;
    }

    unsigned long now = millis();
    if (filteredFreq < minFreqToOperate) {
        if (freqBelowMinStart == 0) freqBelowMinStart = now;
        if (now - freqBelowMinStart >= minFreqTimeoutSec * 1000) inPark = true;
    } else {
        freqBelowMinStart = 0;
        inPark = false;
    }
}

// =================== BERÄKNA SERVOPOSITION ===================
void calculateTargetPosition() {
    if (inPark) {
        targetServoPos = PARK;
        return;
    }

    float freqNorm = max(0.0f, filteredFreq - minFreqToOperate);

    // Potentiometerfaktor ±25%, säker gräns 0.25–1.75
    float potCurveFactor = 1.0 + readPotPercent(potCurve);
    potCurveFactor = constrain(potCurveFactor, 0.25, 1.75);

    float curveFactor = min(freqNorm*freqNorm / (CURVE_INTERNAL / potCurveFactor), MAX_ANGLE - BASE_ANGLE);
    float pos = BASE_ANGLE + curveFactor + OFFSET;

    if (invertServo) pos = MAX_ANGLE - (pos - BASE_ANGLE);
    targetServoPos = constrain(pos, BASE_ANGLE, MAX_ANGLE);
}

// =================== DEBUG ===================
void printDebug() {
    float potCurveFactor = 1.0 + readPotPercent(potCurve);
    potCurveFactor = constrain(potCurveFactor, 0.25, 1.75);

    // Curve i Hz tar hänsyn till OFFSET
    float effectiveCurveHz = sqrt(CURVE_INTERNAL * (MAX_ANGLE - BASE_ANGLE - OFFSET) / potCurveFactor);

    Serial.print("Freq:"); Serial.print(filteredFreq,1);
    Serial.print("  Target:"); Serial.print(targetServoPos);
    Serial.print("  Pos:"); Serial.print(currentServoPos);
    Serial.print("  BASE:"); Serial.print(BASE_ANGLE);
    Serial.print("  MAX:"); Serial.print(MAX_ANGLE);
    Serial.print("  Curve:"); Serial.print(effectiveCurveHz,1); // Endast Curve i Hz
    Serial.print("  OFFSET:"); Serial.println(OFFSET,1);
}

// =================== LOOP ===================
void loop() {
    readPots();
    checkPark();
    calculateTargetPosition();
    updateServoSmooth();
    printDebug();
}