#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>

// =================== ANVÄNDARPARAMETRAR ===================

// =================== FREKVENS OCH RPM KONFIGURATION ===================
// VIKTIGT: Ändra dessa värden baserat på din applikation

// Antal pulser per varv från sensorn
// Range: 1-100 (t.ex. 1 för enkelpuls, 2 för dubbelpuls, 60 för tandhjul etc.)
const int pulsesPerRevolution = 4;

// Max förväntad frekvens (Hz) som ska visas på skärmens bredd
// Detta styr både displayens X-axel och servo-kurvans räckvidd
// Range: 10-300 Hz
// Exempel: För 3500 RPM med 1 puls/varv: 3500/60 = 58 Hz
//          För 3500 RPM med 2 pulser/varv: (3500*2)/60 = 117 Hz
const float maxDisplayFreqHz = 200;

// SERVO INVERTERING
// Byt servorörelsens riktning om det monteras upp-och-ner
// Range: true/false
const bool invertServo = true;

// SERVO MEKANISKA GRÄNSER
// Definierar servots fysiska rörelseområde för att förhindra mekanisk skada
// Range: 0-180 grader
const int minDeflection = 5;      // Minsta tillåtna vinkel
const int maxDeflection = 175;    // Största tillåtna vinkel

// SERVO POSITIONER (före potentiometerjustering)
// Grundinställningar som justeras av potentiometrarna
// Range: minDeflection till maxDeflection
const int BASE_ANGLE_DEFAULT = 80;    // Startposition vid låg frekvens
const int MAX_ANGLE_DEFAULT = 150;    // Position vid maxfrekvens
const int PARK_ANGLE_DEFAULT = 5;     // Viloläge när systemet är avstängt

// FREKVENSKALIBRERING
// Range: -1.0 till 1.0 (multipliceras med positionsområdet)
const float OFFSET_DEFAULT = 0.0;     // Grundförskjutning av hela kurvan

// FREKVENSKÄNSLIGHET
// Range: 0.1 - 500 Hz
const float minFreqToOperate = 2.0;   // Lägsta frekvens för att lämna parkläge

// Referensfrekvens för full utslag - anpassas automatiskt till maxDisplayFreqHz
// Du kan ändra denna manuellt om du vill att full utslag ska ske vid lägre frekvens än max
// Range: minFreqToOperate till maxDisplayFreqHz
const float CURVE_MANUAL_HZ = 180;      // Sätt till 0 för automatisk anpassning, annars önskat värde
// Beräknad kurvreferens (används internt)
const float CURVE_DEFAULT_HZ = (CURVE_MANUAL_HZ > 0 && CURVE_MANUAL_HZ <= maxDisplayFreqHz) 
                                ? CURVE_MANUAL_HZ 
                                : maxDisplayFreqHz;

// TIMEOUT INSTÄLLNINGAR
// Range: 0.1 - 10 sekunder
const float minFreqTimeoutSec = 0.1;  // Tid under minfrekvens innan parkering

// FREKVENS TIMEOUT
// Om ingen puls kommer inom denna tid, nollställs frekvensen
// Range: 100-2000 millisekunder
const unsigned long freqTimeoutMs = 500;  // Nollställ efter 0.5 sekunder utan puls

// SERVO MJUKHET OCH HASTIGHET
// Range: 1-45 grader/steg (högre = snabbare men ryckigare)
const int maxServoStep = 8;           // Max gradförändring per uppdatering vid normal drift

// Range: 1-100 grader/steg (används vid park->base övergång)
const int maxServoStepParkExit = 25;  // Snabbare steg när man lämnar park

// Range: 1-100 grader/steg (används vid boost-funktion)
const int maxServoStepBoost = 15;     // Snabbare steg när boost aktiveras/deaktiveras

// Range: 1-50 millisekunder (lägre = mjukare men långsammare)
const unsigned long servoIntervalMs = 1;  // Tid mellan servouppdateringar

// FREKVENSFILTRERING
// Styr hur snabbt systemet reagerar på frekvensändringar
// Range: 1-10 (1=långsam/stabil, 10=snabb/känslig)
const byte RESPONSIVITY = 3;

// ADAPTIV RESPONSIVITET VID LÅGA FREKVENSER
// Range: 1-10 (högre = snabbare respons vid låga frekvenser)
const byte LOW_FREQ_BOOST = 9;        // Minskar filtrering under 10 Hz för snabbare nollställning

// MANUELL BOOST-FUNKTION
// Tillfällig ökning av servoposition när knapp hålls intryckt
// Range: 0-100% (procent av tillgängligt rörelseområde)
const float manualBoostPercent = 25;  // Ökning när boost-knapp hålls inne

// =================== HÅRDVARUKONFIGURATION ===================

// Potentiometrar för realtidsjustering
const int potBase = A0;     // Justerar BASE_ANGLE
const int potMax = A1;      // Justerar MAX_ANGLE
const int potCurve = A2;    // Justerar kurvans lutning
const int potOffset = A3;   // Justerar OFFSET

// Digitala in/utgångar
#define PULSE_PIN 21        // Frekvensignalens ingång
#define ONOFF_PIN 8         // På/av-knapp (aktivt låg)
#define BOOST_PIN 10        // Manuell boost-knapp (aktivt låg)
#define SERVO_PIN 9         // Servostyrning

// Nokia 5110 Display (SPI)
#define RST_PIN 6
#define CE_PIN  7
#define DC_PIN  5
#define DIN_PIN 11
#define CLK_PIN 13

// =================== OBJEKTINSTANSER ===================

Adafruit_PCD8544 display(CLK_PIN, DIN_PIN, DC_PIN, CE_PIN, RST_PIN);
Servo servo;

// =================== GLOBALA VARIABLER ===================

// Frekvensberäkning (hanteras i interrupt)
volatile unsigned long lastEdgeMicros = 0;
volatile float filteredFreq = 0;

// Timeout-hantering för frekvens
unsigned long lastPulseMillis = 0;

// Servopositionering
int currentPos = -1;        // Aktuell position (-1 = ej initierad)
int targetPos = 0;          // Målposition

// Systemtillstånd
bool inPark = true;         // True = parkläge aktivt
bool boostActive = false;   // True = manuell boost aktiverad
unsigned long freqBelowMinStart = 0;
unsigned long lastServoUpdate = 0;

// Dynamiska positioner (uppdateras av potentiometrar)
int BASE_ANGLE_PHYSICAL;
int MAX_ANGLE_PHYSICAL;
int PARK_PHYSICAL;
float OFFSET;

// =================== HJÄLPFUNKTIONER ===================

/**
 * Beräknar RPM från frekvens och pulser per varv
 */
float calculateRPM() {
    if (pulsesPerRevolution == 0) return 0;
    return (filteredFreq * 60.0) / pulsesPerRevolution;
}

/**
 * Läser en potentiometer och konverterar till procentuell avvikelse
 * Returnerar: -0.25 till +0.25 (25% justering åt vardera hållet)
 * invertDirection: om true, inverterar riktningen
 */
float readPotPercent(int pin, bool invertDirection = false) {
    float value = constrain((analogRead(pin) - 512.0) / 512.0, -1.0, 1.0) * 0.25;
    return invertDirection ? -value : value;
}

/**
 * Inverterar servovinkeln om invertServo är true
 */
int invert(int value) {
    return invertServo ? 180 - value : value;
}

/**
 * Uppdaterar fysiska positioner baserat på potentiometrar
 * Körs varje loop-iteration för realtidsjustering
 */
void updatePhysicalPositions() {
    float basePot = 1 + readPotPercent(potBase);
    float maxPot = 1 + readPotPercent(potMax);
    float offsetPot = readPotPercent(potOffset);

    BASE_ANGLE_PHYSICAL = constrain(
        BASE_ANGLE_DEFAULT * basePot,
        minDeflection,
        maxDeflection - 1
    );

    MAX_ANGLE_PHYSICAL = constrain(
        MAX_ANGLE_DEFAULT * maxPot,
        BASE_ANGLE_PHYSICAL + 1,
        maxDeflection
    );

    PARK_PHYSICAL = PARK_ANGLE_DEFAULT;
    OFFSET = OFFSET_DEFAULT + offsetPot * (MAX_ANGLE_PHYSICAL - BASE_ANGLE_PHYSICAL);

    // Uppdatera målposition om i park eller låg frekvens
    if (inPark || filteredFreq <= minFreqToOperate) {
        targetPos = PARK_PHYSICAL;
    } else {
        targetPos = BASE_ANGLE_PHYSICAL;
    }

    // Initialisera currentPos första gången
    if (currentPos == -1) {
        currentPos = targetPos;
    }
}

// =================== FREKVENSAVLÄSNING (INTERRUPT) ===================

/**
 * Interrupt-rutin som triggas vid varje stigande flank på PULSE_PIN
 * Beräknar frekvens och applicerar exponentiellt glidande medelvärde
 * Använder adaptiv filtrering - MINDRE filtrering vid låga frekvenser för snabbare nollställning
 */
void pulseISR() {
    unsigned long now = micros();
    unsigned long period = now - lastEdgeMicros;

    // Filtrera bort för korta pulser (>1 MHz)
    if (period < 1000) return;

    lastEdgeMicros = now;
    lastPulseMillis = millis();  // Uppdatera timeout-timer
    
    float freq = 1e6 / (float)period;
    float newFreq = constrain(freq, 0, 300.0);

    // Adaptiv filtrering - MINSKA filtrering vid låga frekvenser för snabbare nollställning
    byte effectiveResponsivity = RESPONSIVITY;
    if (newFreq < 10.0) {
        effectiveResponsivity = LOW_FREQ_BOOST;  // Högre responsivity = mindre filter
    }

    // Exponentiellt glidande medelvärde (EMA)
    // Högre responsivity = mindre filtrering (snabbare respons)
    float alpha = map(effectiveResponsivity, 1, 10, 10, 90) / 100.0;  // INVERTERAD från tidigare
    filteredFreq = filteredFreq * (1.0 - alpha) + newFreq * alpha;
}

// =================== BOOST-FUNKTION ===================

/**
 * Kontrollerar boost-knappens status
 */
void checkBoost() {
    boostActive = !digitalRead(BOOST_PIN);  // Aktivt låg
}

// =================== PARKLÄGESLOGIK ===================

/**
 * Hanterar övergång mellan aktivt läge och parkläge
 * Parkerar servot när:
 * - ON/OFF-knappen är av
 * - Frekvensen varit under minFreqToOperate för minFreqTimeoutSec sekunder
 * - Ingen puls kommit inom freqTimeoutMs (signalförlust)
 */
void checkPark() {
    // Nollställ frekvens om ingen puls kommit inom timeout-perioden
    if (millis() - lastPulseMillis > freqTimeoutMs) {
        filteredFreq = 0;
    }
    
    // Tvinga parkläge om knappen är av
    if (!digitalRead(ONOFF_PIN)) {
    inPark = true;
    // filteredFreq = 0;  <-- TA BORT DENNA RAD
    currentPos = targetPos = PARK_PHYSICAL;
    servo.write(invert(currentPos));
    delay(50);
    return;
}

    unsigned long now = millis();

    // Timeout-logik för automatisk parkering
    if (filteredFreq < minFreqToOperate) {
        if (freqBelowMinStart == 0) {
            freqBelowMinStart = now;
        }
        if (now - freqBelowMinStart >= minFreqTimeoutSec * 1000) {
            inPark = true;
        }
    } else {
        // Lämna parkläge när frekvens är tillräckligt hög
        if (inPark) {
            targetPos = BASE_ANGLE_PHYSICAL;
        }
        inPark = false;
        freqBelowMinStart = 0;
    }
}

// =================== SERVOSTYRNING ===================

/**
 * Beräknar målposition baserat på aktuell frekvens och potentiometrar
 * Använder linjär interpolation med justerbar lutning (curve)
 * Lägger till boost om boost-knapp är intryckt
 */
void calculateTargetPosition() {
    if (inPark) {
        targetPos = PARK_PHYSICAL;
        return;
    }

    // Kurv-potentiometern justerar kurvans lutning (inverterad för intuitivitet)
    float potCurveFactor = constrain(1 + readPotPercent(potCurve, true), 0.25, 1.75);

    // Normaliserad frekvens (nollställd vid minFreqToOperate)
    float freqNorm = max(0.0f, filteredFreq - minFreqToOperate);

    // Beräkna delta från basvinkel
    float delta = freqNorm * (MAX_ANGLE_PHYSICAL - BASE_ANGLE_PHYSICAL) *
                  potCurveFactor / (CURVE_DEFAULT_HZ - minFreqToOperate);

    // Slutlig position med offset
    float pos = BASE_ANGLE_PHYSICAL + delta + OFFSET;
    targetPos = constrain(pos, BASE_ANGLE_PHYSICAL, MAX_ANGLE_PHYSICAL);

    // Lägg till boost om knapp är intryckt
    if (boostActive) {
        int boostAmount = (MAX_ANGLE_PHYSICAL - BASE_ANGLE_PHYSICAL) * (manualBoostPercent / 100.0);
        targetPos = constrain(targetPos + boostAmount, BASE_ANGLE_PHYSICAL, maxDeflection);
    }
}

/**
 * Uppdaterar servot mjukt mot målpositionen
 * Begränsar hastighet med maxServoStep för att undvika ryck
 * Använder snabbare steg när man lämnar parkläge eller aktiverar boost
 */
void updateServoSmooth() {
    if (millis() - lastServoUpdate < servoIntervalMs) return;
    lastServoUpdate = millis();

    if (currentPos != targetPos) {
        // Välj lämplig steglängd baserat på situation
        int stepSize = maxServoStep;
        
        // Snabbare steg vid park-exit
        if (currentPos <= PARK_PHYSICAL + 10 && targetPos > PARK_PHYSICAL + 10) {
            stepSize = maxServoStepParkExit;
        }
        // Snabbare steg vid boost aktivering/deaktivering
        else if (boostActive || abs(targetPos - currentPos) > maxServoStep * 3) {
            stepSize = maxServoStepBoost;
        }

        // Begränsa steglängd för mjuk rörelse
        currentPos += constrain(targetPos - currentPos, -stepSize, stepSize);
        currentPos = constrain(currentPos, minDeflection, maxDeflection);
        servo.write(invert(currentPos));
    }
}

// =================== DISPLAY-FUNKTIONER ===================

/**
 * Konverterar frekvens till X-koordinat på displayen
 */
int mapFreqToX(float f) {
    return map(f, 0, maxDisplayFreqHz, 0, 83);
}

/**
 * Konverterar servoposition till Y-koordinat på displayen
 */
int mapPosToY(float p) {
    return 47 - map(p, 0, 200, 0, 47);
}

/**
 * Ritar responskurvan baserat på aktuella inställningar
 */
void drawCurve() {
    float potCurveFactor = constrain(1 + readPotPercent(potCurve, true), 0.25, 1.75);

    for (int x = 0; x < 84; x++) {
        float freqNorm = max(0.0f, (maxDisplayFreqHz * x / 83.0) - minFreqToOperate);
        float delta = freqNorm * (MAX_ANGLE_PHYSICAL - BASE_ANGLE_PHYSICAL) *
                      potCurveFactor / (CURVE_DEFAULT_HZ - minFreqToOperate);
        float pos = BASE_ANGLE_PHYSICAL + delta + OFFSET;

        if (pos > MAX_ANGLE_PHYSICAL) {
            pos = MAX_ANGLE_PHYSICAL;
        }

        display.drawPixel(x, mapPosToY(pos), BLACK);
    }
}

/**
 * Ritar aktuell position som en cirkel på grafen
 */
void drawCurrentPoint() {
    float pos = inPark ? PARK_PHYSICAL : currentPos;
    display.fillCircle(mapFreqToX(filteredFreq), mapPosToY(pos), 2, BLACK);
}

/**
 * Uppdaterar hela displayen med graf, position och status
 */
void updateDisplay() {
    display.clearDisplay();

    // Rita graf och position
    drawCurve();
    drawCurrentPoint();

    // Rad 1: RPM-visning (övre vänster)
    display.setCursor(0, 0);
    float rpm = calculateRPM();
    if (rpm < 10000) {
        display.print(rpm, 0);
    } else {
        display.print(rpm / 1000.0, 1);
        display.print("k");
    }
    display.print(" RPM");

    // Rad 2: Offset i procent
    display.setCursor(0, 40);
    display.print("O:");
    display.print((OFFSET / (MAX_ANGLE_PHYSICAL - BASE_ANGLE_PHYSICAL)) * 100.0, 0);
    display.print("%");

    // Parklägesstatus (höger sida)
    if (inPark) {
        display.setCursor(60, 40);
        display.print("PARK");
    }
    
    // Boost-indikator (nedre höger)
    if (boostActive) {
        display.setCursor(54, 40);
        display.print("BOOST");
    }

    display.display();
}

// =================== SETUP OCH HUVUDLOOP ===================

void setup() {
    // Konfigurera pins
    pinMode(ONOFF_PIN, INPUT_PULLUP);
    pinMode(BOOST_PIN, INPUT_PULLUP);
    pinMode(PULSE_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(PULSE_PIN), pulseISR, RISING);

    // Initiera servo
    servo.attach(SERVO_PIN);

    // Initiera display
    display.begin();
    display.setContrast(50);
    display.clearDisplay();
    display.display();

    // Sätt initial position och timeout-timer
    lastPulseMillis = millis();
    updatePhysicalPositions();
    servo.write(invert(currentPos));
}

void loop() {
    updatePhysicalPositions();
    checkBoost();
    checkPark();
    calculateTargetPosition();
    updateServoSmooth();
    updateDisplay();
}