#include "Arduino.h"
#include "MQ2GasSensorNonBlocking.h"

const float MQ2GasSensorNonBlocking::SMOKE_CURVE[] = { 2.3, 0.53, -0.44 };

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

MQ2GasSensorNonBlocking::MQ2GasSensorNonBlocking(byte sensorAnalogPIN, byte sensorDigitalPIN) :
		MQ2GasSensorNonBlocking(sensorAnalogPIN, sensorDigitalPIN, nullptr, DEFAULT_GAS_SENSE_WARMUP_TIME, SMOKE_CURVE) {
}

MQ2GasSensorNonBlocking::MQ2GasSensorNonBlocking(byte sensorAnalogPIN, byte sensorDigitalPIN, LCDHelper *lcdHelper) :
		MQ2GasSensorNonBlocking(sensorAnalogPIN, sensorDigitalPIN, lcdHelper, DEFAULT_GAS_SENSE_WARMUP_TIME, SMOKE_CURVE) {
}

MQ2GasSensorNonBlocking::MQ2GasSensorNonBlocking(byte sensorAnalogPIN, byte sensorDigitalPIN, LCDHelper *lcdHelper, unsigned long sensorWarmupTime, const float *smokeCurve) :
		sensorAnalogPIN(sensorAnalogPIN), sensorDigitalPIN(sensorDigitalPIN), lcdHelper(lcdHelper), sensorWarmupTime(sensorWarmupTime), smokeCurve(smokeCurve), loadRes(
				DEFAULT_LOAD_RES), airFactor(DEFAULT_AIR_FACTOR), sensorState(SENSOR_STATE_NOT_CALIBRATED), sensorValue(0), baselineResistance(NAN), currResistance(NAN), readSamples(
				SAMPLES_COUNT), readInterval(SAMPLE_INTERVAL), measuringResistance(false), measuredResistance(NAN), currInterval(0), currSample(0), currSamplesCount(0), lastResistanceMeasurementTime(0), currADCValue(0)  {
	instance = this;
	onDigitalInputTriggeredCallback = nullptr;
}

/**
 * This function will attach the object to the inputs given during creation, ie. the analog pin and digital pin as INPUT
 */
void MQ2GasSensorNonBlocking::attach() {
	pinMode(sensorAnalogPIN, INPUT);
	pinMode(sensorDigitalPIN, INPUT_PULLUP);
	attachInterrupt(digitalPinToInterrupt(sensorDigitalPIN), digitalInputTriggered, FALLING);
}

/**
 * This function will be triggered by sensor digital input
 */
void MQ2GasSensorNonBlocking::digitalInputTriggered() {
	if (instance && instance->onDigitalInputTriggeredCallback) {
		instance->onDigitalInputTriggeredCallback();
	}
}

/**
 * Calibrates the sensor in clean air to establish the baseline resistance.
 * Reads multiple sensor values, calculates the average resistance, and adjusts it using the air factor.
 * 
 * @return True when calibration did required amount of sample. This does not guarantee that the value of baseline resistance will be valid.
 */
bool MQ2GasSensorNonBlocking::calibrateSensor() {
	//TODO: finish getSensorResistance non-blocking implementation
	float resistance = 0;

	bool bResult = getSensorResistance(CALIBRATION_SAMPLES_COUNT, CALIBRATION_SAMPLE_INTERVAL, &resistance); // Calculate the average resistance over 50 samples with 500 ms intervals
	if (bResult) {
		if (!isnan(resistance)) {
			baselineResistance = resistance / airFactor;   // Adjust the resistance using the air factor
			//possible fault or sensor not connected when resistance is 0
			if (baselineResistance == 0) {
				baselineResistance = NAN;
			}
		} else {
			baselineResistance = NAN;
		}
		return true;
	}

	return false;
}

/**
 * Calculates the sensor resistance by reading the analog input multiple times.
 * Uses the load resistance and the ADC value to compute resistance.
 * 
 * @param samples Number of samples to take for averaging.
 * @param interval Delay (in milliseconds) between each sample.
 * @param resistance The measured average resistance value of the sensor (in kΩ).
 * @return true if resistance was measured - if it did last sample and updated the value of resistance, otherwise false.
 *
 */
bool MQ2GasSensorNonBlocking::getSensorResistance(byte samples, word interval, float* resistance) {
	float res = 0;
	unsigned long currTime = millis();

	//if called and resistance is note being measured, start measuring
	if (!measuringResistance) {
		measuringResistance = true;
		lastResistanceMeasurementTime = 0;
		currInterval = interval;		//store current measurement interval
		currSample = 1;					//reset current sample counter
		currSamplesCount = samples;			//store how many samples to do
		measuredResistance = 0;
	}

	if (measuringResistance) {
		//should we still sample?
		if (currSample <= currSamplesCount) {
			//if time to sample, measure the resistance
			if ((currTime - lastResistanceMeasurementTime) > currInterval) {
				currADCValue = analogRead(sensorAnalogPIN); // Read the analog value from the sensor
				measuredResistance += currADCValue > 0 ? ((float) loadRes * (1023 - currADCValue) / currADCValue) : 0;    // Calculate resistance

				//count samples measured
				currSample++;
				lastResistanceMeasurementTime = currTime;
				//did we finished sampling?
				if (currSample == currSamplesCount) {
					//return resistance
					measuredResistance /= currSamplesCount;
					*resistance = measuredResistance;
					measuringResistance = false;		//set not measuring for next call

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

						if (DEBUG_TIME_ELAPSED(1500, lastDebugTime)) {

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

							DEBUG_PRINT_F(F("Measured resistance: %s kOhm"), valueStr);

							dtostrf(currResistance, 0, 2, valueStr);

							DEBUG_PRINT_F(F(", curr resistance: %s kOhm"), valueStr);

							dtostrf(baselineResistance, 0, 2, valueStr);

							DEBUG_PRINTLN_F(F(", baseline resistance: %s kOhm"), valueStr);

							DEBUG_RESET_TIME(lastDebugTime);
						}

					#endif*/


					return true;
				}
			}
		}
	}

	//set result to NAN
	*resistance = NAN;
	return false;		//return false because we did not update the resistance
}

bool MQ2GasSensorNonBlocking::isSensorCalibrated() const {
	return sensorState == SENSOR_STATE_CALIBRATED;
}

word MQ2GasSensorNonBlocking::getSensorValue() const {
	return currADCValue;
}

word MQ2GasSensorNonBlocking::getSensorValuePPM() const {
	// Calculate the smoke concentration (ppm) using the logarithmic sensor curve
	if (sensorState == SENSOR_STATE_CALIBRATED && !isnan(currResistance)) {
		return (word) pow(10, (((log(currResistance) - smokeCurve[1]) / smokeCurve[2]) + smokeCurve[0]));
	} else {
		return 0;  // or some other error handling logic
	}
}

/**
 * This function measures sensor resistance and stores the last successfully measured value. This value can be accessed using the {@link #getSensorResistance() }.
 * The functions measures the resistance by taking X samples between specified time interval between them. Those were specified in the constructor.
 */
void MQ2GasSensorNonBlocking::measureSensorResistance() {
	// Measure the sensor resistance over 5 samples with 50 ms intervals
	if (sensorState == SENSOR_STATE_CALIBRATED) {
		float resistance;

		bool bResult = getSensorResistance(readSamples, readInterval, &resistance);
		if (bResult) {
			if (!isnan(resistance)) {
				if (!isnan(baselineResistance)) {
					currResistance = resistance / baselineResistance;
				} else {
					currResistance = NAN;
				}
			}
		}
	}
}

/**
 * Return the value currResistance. Note that this is "last measured value" and not real-time value at the time
 * of calling this. Use {@link #measureSensorResistance()} to measure the resistance first.
 */
float MQ2GasSensorNonBlocking::getSensorResistance() const {
	return currResistance;
}

float MQ2GasSensorNonBlocking::getSensorBaselineResistance() const {
	return baselineResistance;
}

byte MQ2GasSensorNonBlocking::getSensorState() const {
	return sensorState;
}

byte MQ2GasSensorNonBlocking::calibrateGasSensor() {
	if ((sensorState != SENSOR_STATE_CALIBRATED) && (millis() >= sensorWarmupTime)) {
		if (lcdHelper) {
			lcdHelper->printTextToLCD("Calibrating", 0);
			lcdHelper->printTextToLCD("gas sensor...", 1);
			sensorState = SENSOR_STATE_CALIBRATING;
		}

		// Perform sensor calibration to determine baseline resistance
		if (calibrateSensor()) {
			if (lcdHelper) {
				lcdHelper->printTextToLCD("Calibration done", 0);

				//convert resistance to string
				char valueStr[10];
				dtostrf(baselineResistance, 0, 2, valueStr);

				if (!isnan(baselineResistance)) {
					lcdHelper->displayFloatValue("Res=", baselineResistance, STR_UNIT_KOHM, 1, 1);
					DEBUG_PRINTLN_F(F("Gas sensor calibrated! Res=%s %s"), valueStr, STR_UNIT_KOHM);
				} else {
					lcdHelper->printFormattedTextToLCD(1, "Res=%f", baselineResistance);
					//DEBUG_PRINTLN_F(F("Gas sensor calibrated but value is invalid! Res=%s"), valueStr);
				}
			}

			//consider sensor calibrated event thought if value was not read for now
			sensorState = SENSOR_STATE_CALIBRATED;
		}
	}

	return sensorState;
}

void MQ2GasSensorNonBlocking::onDigitalInputTriggered(void (*callback)()) {
	onDigitalInputTriggeredCallback = callback;
}

unsigned long MQ2GasSensorNonBlocking::getSensorWarmupTime() const {
	return sensorWarmupTime;
}

void MQ2GasSensorNonBlocking::setSensorWarmupTime(unsigned long warmupTime) {
	sensorWarmupTime = warmupTime;
}

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