// Input pin constants
#define boostSensor A0 // MAP sensor
#define potPin1 A1     // Boost threshold (0-41psi), If PWM used as Start PSI
#define potPin2 A2     // Only used if PWM selection, used at Full PSI
#define levelSensor A3 // Tank level sensor (2.49k ohm bias resistor)
#define powerSwitch 2  // Arm toggle switch
#define solenoid 9     // Pneumatic valve
#define pump 10        // Pump (timer used)
#define pumpFeedback 11
#define LED 13 // LED

// Pre-compile customizable constants
const uint8_t mapSensorMax = 41;        // Set MAP pressure 5V MAX in psi
const uint16_t debounceDuration = 2000; // 2 seconds debounce time for low level sensor
const uint8_t minDutyCycle = 10;        // PWM duty cycle starting point
const uint8_t lowTankLevel = 10;        // Percentage tank is considered low

// Variables
static bool ledState = LOW; // LED state
bool powerState = LOW;      // System power
bool levelState = HIGH;     // Level sensor
uint8_t boostStart;         // Boost threshold in psi
uint8_t boostStop;          // Boost stop in psi
uint16_t boostPressure;     // Boost pressure in psi
bool useAnalogLevel;        // Analog Level sensor flag
uint8_t tankLevel;          // Analog level sensor percentage
uint16_t levelLowTime;      // Debounce timer
bool usePWMPump;            // PWM usage flag
float dutyCycle;            // Current PWM duty cycle

void setup()
{
    // Initialize output pins
    DDRB |= (1 << DDB5) | (1 << DDB3) | (1 << DDB2); // LED (PB5), pump (PB3), solenoid (PB2)

    // Initialize digital input pins
    DDRD &= ~(1 << DDD2); // powerSwitch (PD2)

    // Setup Timer1 for fast PWM (OC1B, pin 10) in 10-bit mode
    TCCR1A = 0; // Clear Timer1 control registers
    TCCR1B = 0;
    TCNT1 = 0;                            // Clear Timer1 counter
    TCCR1A &= ~(1 << COM1B1);             // Turn off PWM
    TCCR1B |= (1 << WGM12) | (1 << CS11); // Prescaler 8
    OCR1B = 0;                            // Set initial PWM duty cycle

    // Setup Timer2 for regular interrupts
    TCCR2A = 0; // Clear Timer2 control registers
    TCCR2B = 0;
    TCNT2 = 0;               // Clear Timer2 counter
    OCR2A = 249;             // Set compare match register for 1ms intervals (assuming 16MHz clock and 64 prescaler)
    TCCR2A |= (1 << WGM21);  // CTC mode
    TCCR2B |= (1 << CS22);   // Set prescaler to 64 and start Timer2
    TIMSK2 |= (1 << OCIE2A); // Enable Timer2 compare interrupt
    sei();                   // Enable global interrupts

    delay(250); // Startup delay
}

// Timer2 interrupt service routine (ISR)
ISR(TIMER2_COMPA_vect)
{
    // Check if power on, tank level good and within MAP sensor range to spray.
    if ((powerState == HIGH && levelState == HIGH) && (boostPressure >= boostStart))
    {
        digitalWrite(LED, HIGH); // Turn on LED to indicate spraying

        if (usePWMPump) // Check if PWM ramping is needed
        {
            if (boostPressure >= boostStart && boostPressure <= boostStop)
            {
                // Calculate linearly interpolated duty cycle based on boost pressure
                dutyCycle = map(boostPressure, boostStart, boostStop, 10, 100); // Map linear duty cycle based on boost pressure
                dutyCycle = constrain(dutyCycle, 0, 100);                       // Bound the dutyCycle to be between 10 and 100
                TCCR1A = (1 << WGM11) | (1 << WGM10) | (1 << COM1B1);           // non-inverted mode
                OCR1B = (dutyCycle * 1023) / 100;                               // Update PWM duty cycle for pin 10
                digitalWrite(solenoid, HIGH);                                   // Turn on the solenoid
            }
        }

        // Activate pump and solenoid if no PWM selection
        if (!usePWMPump) // Spraying will occur at at the value of StartPSI
        {
            digitalWrite(pump, HIGH);
            digitalWrite(solenoid, HIGH);
        }
    }
    else
    {
        // Ensure everything is off when not spraying
        shutdown();
    }
}

void loop()
{
    // Check for system ready state
    powerState = digitalRead(powerSwitch); // check if system is armed:
    if (powerState == LOW)
    {
        shutdown(); // Always prefer off state for safety
        delay(500); // pause 500ms
        return;     // don't do anything else until system is armed
    }

    uint16_t boostAnalogValue = analogRead(boostSensor);        // Read the analog value from the boost sensor
    boostPressure = (boostAnalogValue * mapSensorMax) / 1023.0; // Convert the analog value to psi (THIS IS WRONG, NEEDS FIXING)
    boostPressure = constrain(boostPressure, 0, mapSensorMax);  // Constrain reading between 0 and MAP sensor max value

    uint16_t startPSI = analogRead(potPin1);             // Read the potentiometer value and map it to the desired boost threshold range
    boostStart = (startPSI * mapSensorMax) / 1023;       // 1 to 41 PSI range
    boostStart = constrain(boostStart, 2, mapSensorMax); // Constrain reading between 0 and MAP sensor max value

    uint16_t stopPSI = analogRead(potPin2);            // Read the potentiometer value and map it to the desired boost threshold range
    boostStop = (stopPSI * mapSensorMax) / 1023;       // 1 to 41 PSI range
    boostStop = constrain(boostStop, 0, mapSensorMax); // Constrain reading between 0 and MAP sensor max value

    // If StartPSI is above StopPSI, PWM is turned off
    // Spraying will occur at at the value of StartPSI
    if (boostStart < boostStop)
    {
        usePWMPump = true;
    }
    else
    {
        usePWMPump = false;
    }

    //    Logic for level sensor
    float tankAnalogValue = analogRead(levelSensor);
    tankLevel = (tankAnalogValue * 100) / 1023; // Analog value to percent

    if (tankLevel <= lowTankLevel) // Handles tank level debounce (slosh)
    {
        if (levelLowTime == 0)
        {
            levelLowTime = millis(); // Record the time when the level sensor first reads low
        }
        else if ((millis() - levelLowTime) >= debounceDuration)
        {
            levelState = LOW; // Set tank level to low
            shutdown();

            // When tank is low, LED will blink
            static unsigned long previousMillis = 0;
            unsigned long currentMillis = millis();
            if ((currentMillis - previousMillis >= 2000 && ledState == LOW) || (currentMillis - previousMillis >= 500 && ledState == HIGH))
            {
                previousMillis = currentMillis;
                ledState = !ledState;
                digitalWrite(LED, ledState);
            }
        }
    }
    else
    {
        levelState = HIGH; //
        levelLowTime = 0;  // Reset the timer if the level sensor reads high
    }
}

// Function to turn off all outputs for safety
void shutdown()
{
    digitalWrite(LED, LOW);
    digitalWrite(solenoid, LOW);
    digitalWrite(pump, LOW);
    TCCR1A &= ~(1 << COM1B1); // Turn off PWM
    OCR1B = 0;                // Set PWM duty cycle to 0 when boostPressure is below boostStart
    dutyCycle = 0;            // Minimum duty cycle
}