// Library for managing hardware

#include <Arduino.h>
#include "hardware.h"

bool simulating = false;
double simulatedPressure = 0.0;
double inflatePSIPerSecond = 0.0;
double deflatePSIPerSecond = 0.0;
double pressureChangeRate = 0.0;
double deflateOffsetMultiplier = 0.0;
double inflateOffsetMultiplier = 0.0;
double offsetMultiplier = 0.0;

void initSimulation() {
	simulating = true;
	simulatedPressure = (double) random(10, 30);
	inflatePSIPerSecond = 0.0303;
	deflatePSIPerSecond = 0.0958;
	deflateOffsetMultiplier = random(3, 10) / 100.0;
	inflateOffsetMultiplier = random(103, 160) / 100.0;
	offsetMultiplier = 1.0;
}

void tickSimulation() {
	static unsigned long lastTick = millis();

	if (millis() > lastTick + 100) {
		double pressureDelta = (millis() - lastTick) / 1000.0 * pressureChangeRate;

		simulatedPressure += pressureDelta;
		lastTick = millis();
	}
}

void measurePressure(double &pressureValue) {
	if (simulating) {
		double adjusted = simulatedPressure + (random(-100, 100) / 100.0);
		adjusted *= offsetMultiplier;
		pressureValue = 0.99 * pressureValue + 0.01 * adjusted;
	} else {
		int sensorValue = analogRead(PRESSURE_SENSOR_PIN);
		double adjusted = 0.098 * sensorValue - 16.56 + 3.58;
		pressureValue = 0.99 * pressureValue + 0.01 * adjusted;
	}
}

void resamplePressure(double &pressureValue) {
	if (simulating) {
		double adjusted = simulatedPressure + (random(-100, 100) / 100.0);
		adjusted *= offsetMultiplier;
		pressureValue = adjusted;
	} else {
		int sensorValue = analogRead(PRESSURE_SENSOR_PIN);
		double adjusted = 0.098 * sensorValue - 16.56 + 3.58;
		pressureValue = adjusted;
	}
}

void setSoleniod(int solenoidState) {
	if (solenoidState == SOLENOID_STOP) {
		pressureChangeRate = 0.0;
		offsetMultiplier = 1.0;

		digitalWrite(RELAY1_PIN, HIGH);
		digitalWrite(RELAY2_PIN, HIGH);
	} else if (solenoidState == SOLENOID_INFLATE) {
		pressureChangeRate = inflatePSIPerSecond;
		offsetMultiplier = inflateOffsetMultiplier;

		digitalWrite(RELAY1_PIN, LOW);
		digitalWrite(RELAY2_PIN, HIGH);
	} else if (solenoidState == SOLENOID_DEFLATE) {
		pressureChangeRate = -deflatePSIPerSecond;
		offsetMultiplier = deflatePSIPerSecond;

		digitalWrite(RELAY1_PIN, HIGH);
		digitalWrite(RELAY2_PIN, LOW);
	}
}