// Library for managing hardware

#include <Arduino.h>
#include "hardware.h"
#include "XGZP.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;
float value = 1;

XGZP sensor;

void initHardware() {
	Serial.println("Initializing hardware...");

	if(!sensor.begin()){
		Serial.printf("Error initializing Sensor\n Locking up.");
		while(1);
	}
}

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 {
		sensor.read(&value);
		double adjusted = 1.368 * value - 6.4;
		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 {
		sensor.read(&value);
		double adjusted = 1.368 * value - 6.4;
		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);
	}
}