#include "VentilationControl.h"

// Static member initialization
const byte VentilationControl::DEFAULT_DOOR_POSITIONS[] = {0, 30, 60, 90};

// Initialize the static instance pointer
VentilationControl* VentilationControl::instance = nullptr;

//Full constructor
VentilationControl::VentilationControl(byte servoPin, byte fanPWMPIN, byte fanRelayPIN, byte fanRPMPIN, GenericPWM* pwmController, FireWatch* fireWatch,
                                       float tempMaxInside, float tempMaxDiff,
                                       const byte* doorPositions, byte numPositions,
                                       byte pulsesPerFanRevolution,
                                       word fanStuckTreshold)
    : servoPin(servoPin), fanPWMPIN(fanPWMPIN), fanRelayPIN(fanRelayPIN), fanRPMPIN(fanRPMPIN), pwmController(pwmController), fireWatch(fireWatch),
      tempMaxInside(tempMaxInside), tempMaxDiff(tempMaxDiff),
      doorPositions(doorPositions), numPositions(numPositions), pulsesPerFanRevolution(pulsesPerFanRevolution),
      fanStuckTreshold(fanStuckTreshold),
      fanDuty(FAN_OFF_DUTY), fanOn(false), prevTempDiff(0), lastTempDiffUpdateTime(0), lastFanDoorPositionUpdateTime(0), fanDoorSpeed(FAN_DOOR_SPEED),
	  fanRelayPowerOnTime(0), lastFanDuty(0), currentFanDoorServoPulse(PULSE_0_DEGREE) {
    instance = this;
}

// Constructor with defaults
VentilationControl::VentilationControl(byte servoPin, byte fanPWMPIN, byte fanRelayPIN, byte fanRPMPIN, GenericPWM* pwmController, FireWatch* fireWatch)
    : VentilationControl(servoPin, fanPWMPIN, fanRelayPIN, fanRPMPIN, pwmController, fireWatch,
                         DEFAULT_TEMP_MAX_INSIDE, DEFAULT_TEMP_MAX_DIFF,
                         DEFAULT_DOOR_POSITIONS, 4, // <-- This is the numPositions
                         DEFAULT_PULSES_PER_FAN_REVOLUTION, DEFAULT_FANSTUCK_THRESHOLD) {}


/**
 * Will attach fan door servo to fanDoorPIN given in initialization and will init fanPWMPIN for OUTPUT,
 fanRelayPIN for output and will turn fan OFF for start.
 *
 */
void VentilationControl::attachServo() {
    fanDoorServo.attach(servoPin);

	if (fireWatch->getSettingsManager()) {
		int DOOR_PULSE = MIN_PULSE_WIDTH + (PULSE_WIDTH_PER_DEGREE * fireWatch->getSettingsManager()->getLastFanDoorPosition());
		currentFanDoorServoPulse = DOOR_PULSE;
	}
	fanDoorServo.write(currentFanDoorServoPulse); // Initialize to fully closed

    pinMode(fanPWMPIN, OUTPUT);           // Set the fan PWM pin as output
    pinMode(fanRelayPIN, OUTPUT);         // Set the fan relay control pin as output
    pinMode(fanRPMPIN, INPUT_PULLUP);
    //turnFanOff();                         // Ensure the fan starts off
}

/**
 * Attach interrupt for the fan tachometer pin
 */
void VentilationControl::attachFanInterrupt() {
    pinMode(fanRPMPIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(fanRPMPIN), fanRotationSensTriggered, FALLING);
}

/**
 * This function will be called each time a fan rotation sensor generates a signal (goes from HIGH to LOW)
 * A debounce mechanism ensures noise does not trigger false interrupts.
 *
 * Static interrupt handler
 */
static void VentilationControl::fanRotationSensTriggered() {
    /*
    * As the fan rotates, the tachometer pin generates pulses. Each falling edge triggers the tachISR interrupt.
    * The interrupt handler records the timestamps (fanTS1 and fanTS2).
    *
    * If the time difference between consecutive interrupts is less than DEBOUNCE (default 0 ms), the interrupt is ignored to filter out noise.
    *
    * The most recent interrupt updates fanTS2, and the previous fanTS2 value is stored in fanTS1
    *
    * When calcFANRPM() is called, the function checks if the time since the last interrupt (millis() - fanTS2) is less than FANSTUCK_THRESHOLD.
    * If true, it calculates RPM using the time difference between fanTS2 and fanTS1.
    * If no valid interrupt is detected within the threshold, the RPM is reported as 0.
    */
	//DEBUG_PRINTLN_F(F("fanRotationSensTriggered 111"));

    if (instance) {
        unsigned long m = millis();

        if ((m - instance->fanTS2) > DEBOUNCE) {
            //DEBUG_PRINTLN_F(F("fanRotationSensTriggered"));
            instance->fanTS1 = instance->fanTS2;
            instance->fanTS2 = m;
        }
    }
}

/** 
 * Calculates the RPM based on the timestamps of the last 2 interrupts. Can be called at any time.
 * This function calculates RPM based on the time difference between the last two pulses:
 *   (60000 / (fanTS2 - fanTS1)) / pulsesPerFanRevolution
 *
 *  60000 converts milliseconds to minutes.
 * The division by pulsesPerFanRevolution pulses per revolution (adjust if your fan generates a different number of pulses per revolution).
 * 
 * If no interrupt occurs for a period longer than fanStuckTreshold, the fan is considered stuck, and the RPM is reported as 0
 */
word VentilationControl::calcFANRPM() {
    noInterrupts(); // Prevent interrupts while reading volatile variables
    unsigned long ts1 = fanTS1;
    unsigned long ts2 = fanTS2;
    interrupts();
    if ((millis() - ts2) < fanStuckTreshold && ts2 != 0) {
        return (word)((60000 / (ts2 - ts1)) / pulsesPerFanRevolution);
    }
    return 0;
}

/**
 * Function to calculate fan duty (0-100)
 *
 */
byte VentilationControl::calculateFanDuty(float tempDiff, float convergenceRate) {
    if (tempDiff < 0) return FAN_OFF_DUTY; // No fan if difference is negative, meaning inside temp is below treshold

    // Adjust speed based on temperature difference and convergence
    float duty = map(tempDiff, 0, tempMaxDiff, FAN_IDLE_DUTY, FAN_MAX_DUTY);
    duty += convergenceRate > 0 ? convergenceRate * 5 : 0; // Boost duty if temperatures are converging
    return constrain((byte)duty, FAN_IDLE_DUTY, FAN_MAX_DUTY); // Ensure duty stays between MIN_FAN_DUTY and 100
}

/**
 * Function to calculate door position
 *
 */
byte VentilationControl::calculateDoorPosition(float tempDiff) {
    if (tempDiff < 0) return doorPositions[0]; // Fully closed if inside temperature is below max inside temperature
    if (tempDiff > tempMaxDiff) return doorPositions[numPositions - 1]; // Fully open

    // Proportionally determine the position based on the temperature difference,
    //but at least first step, which is 30 deg. If we got here, there is some temp diff, fan will be on so doors need to be opened at least at first position
    byte step = (byte)map(tempDiff, 0, tempMaxDiff, 1, numPositions - 1);
    return doorPositions[step];
}


void VentilationControl::turnFanOn() {
	//only turn fan relay ON when enough time passed since start
	unsigned long currTime = millis();

    if (!fanOn && (currTime > FAN_ON_DELAY)) {
        fanOn = true;
        digitalWrite(fanRelayPIN, HIGH); // Turn on relay
        fanRelayPowerOnTime = currTime;
    }
}

void VentilationControl::turnFanOff() {
    if (fanOn) {
        fanOn = false;
        if (pwmController) {
           pwmController->setPwmDuty(FAN_OFF_DUTY, fanPWMPIN);
           lastFanDuty = FAN_OFF_DUTY;
        }
        delay(500);
        digitalWrite(fanRelayPIN, LOW); // Turn off relay
        fanRelayPowerOnTime = 0;
    }
}

/**
 * This function automatically controls ventilation fan speed and vent fan doors based on difference
 * of outside temeperature and inside temperature, and on how fast the converge
 *
 */
void VentilationControl::controlVentilation() {
      // Calculate the temperature difference and convergence rate
	float insideTemperature = fireWatch->getDHTSensors()->getInsideTemperature();
	//float outsideTemperature = fireWatch->getDHTSensors()->getOutsideTemperature();
    float tempDiff = NAN;
    float convergenceRate = NAN;
    SettingsManager* settingsManager = fireWatch->getSettingsManager();

    //if there is fire alarm, make sure doors are closed and fan is OFF, so there is as less air circulation as possible helping
    //starving the fire. Also, close doors and turn off fan if warning threshold is triggered and gas sensor has some PPM reading, that is possibly
    //fire developing
	if ((fireWatch->getAlertMode() == FireAlertMode::FIRE_HAZARD || settingsManager->getFireAlertMode() == FireAlertMode::FIRE_HAZARD) ||
			(fireWatch->getAlertMode() == FireAlertMode::FIRE_WARNING && fireWatch->getGasSensor()->getSensorValuePPM() > settingsManager->getAlertPPM())) {
		turnFanOff();
		//modify servo position only if differs from the one it is in
		/*if (fanDoorServo.read() != MIN_FAN_DOOR_ANGLE) {
			fanDoorServo.write(MIN_FAN_DOOR_ANGLE);
		}*/
		setFanDoorPosition(MIN_FAN_DOOR_ANGLE);
		return;
	}

	//compute temperature difference between inside and outside
    if (!isnan(insideTemperature) && !isnan(tempMaxInside)) {
    	//this will be negative until inside temperature is greater than threshold
    	tempDiff = insideTemperature - tempMaxInside;
    	convergenceRate = (tempDiff - prevTempDiff) / ((millis() - lastTempDiffUpdateTime) / 1000.0); // °C/s
    	lastTempDiffUpdateTime = millis();
    	prevTempDiff = tempDiff;
    }


    // Determine fan duty based on temperature difference and convergence rate, if control mode is set to AUTO
    byte fanDuty = FAN_OFF_DUTY;
    // Determine door position based on temperature difference and if not overridden by settings manager
    byte doorPosition = 0;

    //First check if enclosure is in fire warning mode. If so and there is no gas sensor reading, there is just high temperature inside, force fan to max. If there is
    //also gas sensor reading, fire is probably developing, then turn off the fan so it does not help the fire by moving the air and creating the column effect.
    //otherwise, the enclosure is in normal state, then decide whether by set for auto or manual control
    if (fireWatch->getAlertMode() == FireAlertMode::FIRE_WARNING) {
		if (fireWatch->getGasSensor()->getSensorValuePPM() < settingsManager->getAlertPPM()) {
			//in case of warning, but not critical PPM,  override the doors for max open
			fanDuty = FAN_MAX_DUTY;		//vent it out
			doorPosition = MAX_FAN_DOOR_ANGLE;		//vent it out
		} else {
			//if there is PPM detection, fire is probably developing, close the doors
			fanDuty = FAN_OFF_DUTY;
			doorPosition = MIN_FAN_DOOR_ANGLE;
		}
	} else {
		//Logic for determining fan duty when enclosure is in normal state
		if (settingsManager->getTempControlMode() == SettingsManager::AUTO_MODE) {
			if (!isnan(insideTemperature) && !isnan(tempMaxInside)) {
				fanDuty = calculateFanDuty(tempDiff, convergenceRate);

				/*#ifdef MK4SC_DEBUG
					// Debug output
					static unsigned long lastDebugTime = 0;

					if (DEBUG_TIME_ELAPSED(2000, lastDebugTime)) {

						char valueStr[10];
						dtostrf(insideTemperature, 0, 2, valueStr);

						DEBUG_PRINT_F(F("Insd temp: %s °C"), valueStr);

						dtostrf(outsideTemperature, 0, 2, valueStr);

						DEBUG_PRINT_F(F(", Outsd temp: %s °C"), valueStr);

						dtostrf(tempDiff, 0, 2, valueStr);

						DEBUG_PRINT_F(F(", Temp diff: %S °C"), valueStr);

						dtostrf(convergenceRate, 0, 2, valueStr);

						DEBUG_PRINT_F(F(", Convergence Rate: %s °C/s"), convergenceRate);
						DEBUG_PRINTLN_F(F(", Fan Duty: %d \%"), fanDuty);

						DEBUG_RESET_TIME(lastDebugTime);
					}

				#endif*/
			} else {
				//mode is auto, but temperature readings are invalid, thus, cannot control fan based on temperatures
				fanDuty = FAN_OFF_DUTY;
			}
		} else {
			//will return direct fan PWM duty from  FAN_IDLE_DUTY - FAN_MAX_DUTY
			fanDuty = settingsManager->getFanPWM();
		}


		//Logic to get fan door angle when enclosure is in normal state
		if (settingsManager->getFanDoorAngle() == SettingsManager::FAN_DOOR_ANGLE_AUTO) {
			if (!isnan(tempDiff)) {
				doorPosition = calculateDoorPosition(tempDiff);
			} else {
				//no temp diff means inside, outside or both temperatures are invalid
				doorPosition = MIN_FAN_DOOR_ANGLE;
			}
		} else {
			doorPosition = settingsManager->getFanDoorAngle();
		}
	}

    // Fan control logic
    if (fanDuty >= FAN_IDLE_DUTY) {
        if (!fanOn) {
            turnFanOn(); // Turn on the fan relay
        }
        //if fan is on, we have PWM controller and at least 1 second passed since switching relay ON, start setting up PWM
        if (fanOn && pwmController && ((millis() - fanRelayPowerOnTime) > 1000)) {

          if (lastFanDuty != fanDuty) {
        	  pwmController->setPwmDuty(fanDuty, fanPWMPIN);
        	  lastFanDuty = fanDuty;
          }

          /*#ifdef MK4SC_DEBUG
				// Debug output
				static unsigned long lastDebugTime = 0;

				if (DEBUG_TIME_ELAPSED(2000, lastDebugTime)) {

					DEBUG_PRINTLN_F(F("Fan Duty: %d \%"), fanDuty);

					DEBUG_RESET_TIME(lastDebugTime);
				}

			#endif*/
        }
    } else {
        turnFanOff(); // Turn off the fan relay
    }

    //update fan door speed in case it changed
    word currDoorSpeed = settingsManager->getFanDoorSpeed();
    if (currDoorSpeed != fanDoorSpeed) {
    	fanDoorSpeed = currDoorSpeed;
    }

    //set new door position. This will change over time, not instantly
    setFanDoorPosition(doorPosition);
}

bool VentilationControl::isFanOn() const {
  return fanOn;
}

/**
 * This function changes fan door position. It changes the door position over time so the change is not instant.
 */
/*
void VentilationControl::setFanDoorPosition(byte targetPos) {
	int targetPulse = PULSE_0_DEGREE + (PULSE_WIDTH_PER_DEGREE * targetPos);

	//int currDoorPulse = fanDoorServo.read();
	word currDoorPulse = currentFanDoorServoPulse;

	//if doors are not in position
	if (currDoorPulse != targetPulse) {
		unsigned long currTime = millis();

		//and it is time for change
		if ((currTime - lastFanDoorPositionUpdateTime) > fanDoorSpeed) {
			word newDoorPulse = currDoorPulse;

			//determine which way to change to get to target position
			if (currDoorPulse < targetPulse) {
				newDoorPulse += PULSE_WIDTH_PER_DEGREE;
				if (newDoorPulse > PULSE_90_DEGREE) {
					newDoorPulse = PULSE_90_DEGREE;
				}
			} else {
				newDoorPulse -= PULSE_WIDTH_PER_DEGREE;
				if (newDoorPulse < PULSE_0_DEGREE) {
					newDoorPulse = PULSE_0_DEGREE;
				}
			}

			//set the new position
			fanDoorServo.write(newDoorPulse);
			currentFanDoorServoPulse = newDoorPulse;

			lastFanDoorPositionUpdateTime = currTime;

			//if we reached the target pos, set last update time to 0 so next position change can start right away
			if (newDoorPulse == targetPulse) {
				lastFanDoorPositionUpdateTime = 0;

				//store actual fan door position
				fireWatch->getSettingsManager()->setLastFanDoorPosition(targetPos);
			}

			//some debugging
			#ifdef MK4SC_DEBUG
				// Debug output
				static unsigned long lastDebugTime = 0;

				if (DEBUG_TIME_ELAPSED(2000, lastDebugTime)) {
					DEBUG_PRINTLN_F(F("Curr fan door pos: %d -> %d (%d)"), currDoorPulse, newDoorPulse, targetPulse);

					DEBUG_RESET_TIME(lastDebugTime);
				}
			#endif
		}

	}
}
*/

void VentilationControl::setFanDoorPosition(byte targetPos)
{
	word targetPulse = PULSE_0_DEGREE + (PULSE_WIDTH_PER_DEGREE * targetPos);
    const word step = PULSE_WIDTH_PER_DEGREE;  // your step size in µs
    unsigned long currTime = millis();

    // Not time to update yet
    if (currTime - lastFanDoorPositionUpdateTime < fanDoorSpeed)
        return;

    lastFanDoorPositionUpdateTime = currTime;

    if (targetPulse < PULSE_0_DEGREE)  targetPulse = PULSE_0_DEGREE;
    if (targetPulse > PULSE_90_DEGREE) targetPulse = PULSE_90_DEGREE;


    word currDoorPulse = currentFanDoorServoPulse;
    word newDoorPulse = currDoorPulse;

    // --- Determine direction ---
    if (targetPulse > currDoorPulse) {
        // increasing
        word diff = targetPulse - currDoorPulse;
        newDoorPulse = (diff > step) ? currDoorPulse + step : targetPulse;
    }
    else if (targetPulse < currDoorPulse) {
        // decreasing
        word diff = currDoorPulse - targetPulse;
        newDoorPulse = (diff > step) ? currDoorPulse - step : targetPulse;
    }
    else {
        // already at target
        return;
    }

    // --- Final write ---
    fanDoorServo.write(newDoorPulse);
    currentFanDoorServoPulse = newDoorPulse;

    //some debugging
	#ifdef MK4SC_DEBUG
		// Debug output
		static unsigned long lastDebugTime = 0;

		if (DEBUG_TIME_ELAPSED(2000, lastDebugTime)) {
			DEBUG_PRINTLN_F(F("Curr fan door pos: %d -> %d (%d)"), currDoorPulse, newDoorPulse, targetPulse);

			DEBUG_RESET_TIME(lastDebugTime);
		}
	#endif
}



void VentilationControl::setMaxInsideTemp(float fTemp) {
	tempMaxInside = fTemp;
}

void VentilationControl::setMaxTempDifference(float fDiff) {
	tempMaxDiff = fDiff;
}

VentilationControl::~VentilationControl() {
    instance = nullptr;
}
