#include <Arduino.h>

#include "DebugLog.h"
#include "ADC.h"
#include "OTA.h"
#include "DataLogger.h"
#include "utils.h"

#include <WiFi.h>
#include <WiFiMulti.h>
#include <FS.h>
#include <SPIFFS.h>
#include <ESPAsyncWebServer.h>

#include <Dps310.h>

#include "wifi-credentials.h"

#include <limits>

#define DUMMY_DATA 0

AsyncWebServer server(80);

ADC currentSensor(36);
ADC batterySensor(39);
Dps310 pressureSensor = Dps310();
const int8_t speedSensorPin = 13;
const int8_t debugLedPin = 2;
const int8_t I2C_SDA = 15;
const int8_t I2C_SCL = 4;

const float wheelDiameterInches = 20;
const int numImpulsesPerTurn = 2;
const float wheelCircumferenceMeters = wheelDiameterInches * 0.0254f * 3.1415f / (float)numImpulsesPerTurn;
const uint32_t wheelCircumferenceMillimeters = (uint32_t)(wheelCircumferenceMeters * 1000.0f + 0.5f);

uint16_t batteryVoltage = 0; // in mV
uint16_t batteryOutputCurrent = 0; // in mV
int16_t temperature = 0; // in tenth of °C
int32_t altitude = 0; // in mm above sea level (can be negative if below sea level, or depending on atmospheric conditions)

// current trip
uint32_t tripDistance = 0; // in meters
uint16_t tripMovingTime = 0; // cumulated seconds, only when moving at non-zero speed
uint16_t tripTotalTime = 0; // total trip time in seconds
uint32_t tripAscendingElevation = 0; // cumulated ascending elevation, in millimeters
uint32_t tripMotorEnergy = 0; // in Joules

WiFiMulti wifiMulti;
wl_status_t wifi_STA_status = WL_NO_SHIELD;
unsigned long wifiConnexionBegin = 0;
const unsigned long retryWifiConnexionDelay = 60000; // in milliseconds

unsigned long stoppedSince = -1;

volatile bool debugLedState = true;

volatile bool speedSensorState = false;
volatile unsigned long speedSensorRiseTime = 0;
volatile unsigned long speedSensorLastImpulseTime = 0;
volatile unsigned long speedSensorLastImpulseInterval = (unsigned long)-1; // in milliseconds
volatile uint32_t speedSensorDistance = 0; // Cumulated measured distance, in millimeters. This value will overflow after about 4000km.
void IRAM_ATTR onSpeedSensorChange(bool newState)
{
	if(speedSensorState == newState) return;
	unsigned long now = millis();
	speedSensorState = newState;

	bool magnetDetected = !speedSensorState; // the magnet closes the contact which pulls the pin low

	if(magnetDetected)
	{
		speedSensorRiseTime = now;
	}
	else
	{
		unsigned long impulseDuration = utils::elapsed(speedSensorRiseTime, now);
		if(impulseDuration > 500) return; // impulse was too long, ignore it (maybe magnet stopped near the sensor)

		unsigned long timeSinceLastImpulse = utils::elapsed(speedSensorLastImpulseTime, now);

		// TODO: find a simple formula that works for any wheel diameter and number of magnets
		unsigned long minInterval = speedSensorLastImpulseInterval == (unsigned long)-1 ? 200 : std::min((unsigned long)200, (unsigned long)30 + speedSensorLastImpulseInterval / 2);

		if(timeSinceLastImpulse < minInterval)
		{
			// too little time between impulses, probably some bouncing, ignore it
		}
		else
		{
			speedSensorDistance += wheelCircumferenceMillimeters;

			if(timeSinceLastImpulse < 4000)
			{
				speedSensorLastImpulseTime = now;
				speedSensorLastImpulseInterval = timeSinceLastImpulse;
			}
			else
			{
				// too much time between impulses, can't compute speed from that
				speedSensorLastImpulseTime = now;
				speedSensorLastImpulseInterval = (unsigned long)-1;
			}
		}
	}
}

void IRAM_ATTR onSpeedSensorChange() { onSpeedSensorChange(digitalRead(speedSensorPin) == HIGH); }

float getSpeed()
{
	#if DUMMY_DATA
	{
		float result = max(0.0f, sinf((float)millis()/30000.0f)) * 7.0f;
		return result < 0.25f ? 0.0f : result;
	}
	#endif

	unsigned long now = millis();

	noInterrupts();
	unsigned long lastImpulseInterval = speedSensorLastImpulseInterval;
	unsigned long lastImpulseTime = speedSensorLastImpulseTime;
	interrupts();

	unsigned long timeSinceLastImpulse = utils::elapsed(lastImpulseTime, now);

	unsigned long interval = timeSinceLastImpulse > lastImpulseInterval * 10 / 9 ? timeSinceLastImpulse : lastImpulseInterval;

	float speed = wheelCircumferenceMeters / (float)interval * 1000.0f; // in meters per second

	if(speed < 0.25f)
	{
		return 0.0f; // if speed is very low (less than 1km/h) it probably means we've stopped
	}
	return speed;
}

void connectWifi()
{
	wifiMulti = WiFiMulti();

	const int numSSIDs = sizeof(wifi_STA_credentials)/sizeof(wifi_STA_credentials[0]);
	if(numSSIDs > 0)
	{
		DebugLog.println("Connecting to wifi...");

		for(int idx = 0; idx < numSSIDs; ++idx)
		{
			wifiMulti.addAP(wifi_STA_credentials[idx].SSID, wifi_STA_credentials[idx].password);
		}

		wifiConnexionBegin = millis();
		wifiMulti.run();
	}
}

void setup()
{
	pinMode(debugLedPin, OUTPUT);
	digitalWrite(debugLedPin, HIGH);

	pinMode(speedSensorPin, INPUT_PULLUP);
	attachInterrupt(speedSensorPin, &onSpeedSensorChange, CHANGE);

	Serial.begin(115200);

	if(!SPIFFS.begin(false)){
		DebugLog.println("SPIFFS Mount Failed");
		return;
	}

	// Set WiFi mode to both AccessPoint and Station
	WiFi.mode(WIFI_AP_STA);

	// Create the WiFi Access Point
	if(wifi_AP_ssid != nullptr)
	{
		DebugLog.println("Creating wifi access point...");
		WiFi.softAP(wifi_AP_ssid, wifi_AP_password);

		DebugLog.print("Wifi access point created, SSID=");
		DebugLog.print(wifi_AP_ssid);
		DebugLog.print(", IP=");
		DebugLog.println(WiFi.softAPIP());
	}

	// Also connect as a station (if the configured remote access point is in range)
	connectWifi();
	
	OTA.begin();

	Wire.begin(I2C_SDA, I2C_SCL);

	pressureSensor.begin(Wire);

	server.on("/api/debug/log", HTTP_GET, [](AsyncWebServerRequest *request){
		AsyncResponseStream *response = request->beginResponseStream("text/plain");
		int logPartIdx = 0;
		while(true)
		{
			const char* text = DebugLog.get(logPartIdx);
			if(text[0] == 0) break;
			response->print(text);

			++logPartIdx;
		}

		request->send(response);
	});

	server.on("/api/status", HTTP_GET, [](AsyncWebServerRequest *request){
		int v = batteryVoltage;
		int c = batteryOutputCurrent;
		int s = (int)(getSpeed() * 1000.0f + 0.5f);
		int temp = temperature;
		int alt = altitude;

		int td = tripDistance;
		int ttt = tripTotalTime;
		int tmt = tripMovingTime;
		int tae = tripAscendingElevation / 100; // convert mm to dm
		int tme = tripMotorEnergy / 360; // convert Joules to dWh (tenth of Wh)

		const char* logFileName = DataLogger::get().currentLogFileName();
		if(String(logFileName).startsWith("/log/")) logFileName += 5;

		int totalSize = (int)(SPIFFS.totalBytes() / 1000);
		int usedSize = (int)(SPIFFS.usedBytes() / 1000);

		char json[256];
		sprintf(json, "{\"v\":%d,\"c\":%d,\"s\":%d,\"td\":%d,\"ttt\":%d,\"tmt\":%d,\"tae\":%d,\"tme\":%d,\"temp\":%d,\"alt\":%d,\"log\":\"%s\",\"tot\":%d,\"used\":%d}", v, c, s, td, ttt, tmt, tae, tme, temp, alt, logFileName, totalSize, usedSize);
		request->send(200, "text/json", json);
	});

	server.on("/api/log/list", HTTP_GET, [](AsyncWebServerRequest *request){
		String json;

		json = "{\"files\":[";

		auto logFolder = SPIFFS.open("/log");
		auto file = logFolder.openNextFile();
		bool first = true;
		while(file)
		{
			if(!first) json += ",";
			json += "{\"n\":\"/api";
			json += file.name();
			json += "\",\"s\":";
			json += file.size();
			json += "}";

			first = false;
			file = logFolder.openNextFile();
		}

		json += "]}";
		request->send(200, "text/json", json.c_str());
	});

	// Special case to send index.html without caching
	server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){ request->send(SPIFFS, "/www/index.html", "text/html"); });
	server.serveStatic("/index.html", SPIFFS, "/www/index.html");

	// Log files (not cached)
	server.serveStatic("/api/log", SPIFFS, "/log/");

	// Other static files are cached (index.html knows whether to ignore caching or not for each file)
	server.serveStatic("/", SPIFFS, "/www/").setCacheControl("max-age=5184000");

	server.begin();
	DebugLog.println("HTTP server started");

	digitalWrite(debugLedPin, LOW);
}

void handle_wifi_connection()
{
	wl_status_t newWifiStatus = WiFi.status();
	if(newWifiStatus != wifi_STA_status)
	{
		if(newWifiStatus == WL_CONNECTED)
		{
			DebugLog.print("Connected to wifi (");
			DebugLog.print(WiFi.SSID().c_str());
			DebugLog.print("), ip=");
			DebugLog.println(WiFi.localIP());
		}
		else if(newWifiStatus == WL_DISCONNECTED)
		{
			char codeStr[16];
			sprintf(codeStr, "%d", (int)newWifiStatus);
			DebugLog.print("Lost wifi connexion (");
			DebugLog.print(codeStr);
			DebugLog.println(")");

			connectWifi();
		}
		else
		{
			char codeStr[16];
			sprintf(codeStr, "%d", (int)newWifiStatus);
			DebugLog.print("Wifi state: ");
			DebugLog.println(codeStr);
		}

		wifi_STA_status = newWifiStatus;
	}

	if(wifi_STA_status != WL_CONNECTED)
	{
		unsigned long now = millis();
		unsigned long elapsed = utils::elapsed(wifiConnexionBegin, now);
		if(elapsed > retryWifiConnexionDelay)
			connectWifi();
	}
}

void handle_ADC_measures()
{
	const int numSamples = 100;

	float averageV = 0.0f;
	float averageC = 0.0f;
	for(int sample = 0; sample < numSamples; ++sample)
	{
		delay(1);
		float v = batterySensor.read();
		float c = currentSensor.read();

		averageV += v;
		averageC += c;
	}

	averageV /= (float)numSamples;
	averageC /= (float)numSamples;

	if(averageV < 0.2f) averageV = 0.0f;

	averageV *= 27.000f; // account for voltage divider to retrieve the input voltage
	averageC = std::max(0.0f, averageC - 2.5f) / 0.0238f; // convert voltage to current, according to the sensor linear relation

	batteryVoltage = (uint16_t)(averageV * 1000.0f + 0.5f);
	batteryOutputCurrent = (uint16_t)(averageC * 1000.0f + 0.5f);

	#if DUMMY_DATA
	batteryVoltage = (uint16_t)((float)random(4000, 4020) / 100.0f * 1000.0f + 0.5f);
	batteryOutputCurrent = (uint16_t)(max(0.0f, sinf((float)millis()/30000.0f)) * 25.0f * 1000.0f + 0.5f);
	#endif
}

void handle_pressure_measure()
{
	const uint8_t oversampling = 7;
	int16_t ret;

	float temp; // in Celcius degrees
	ret = pressureSensor.measureTempOnce(temp, oversampling);
	if(ret != 0)
	{
		DebugLog.print("Failed to measure temperature: "); DebugLog.println(ret);
		return;
	}
	temperature = (int16_t)(temp * 10.0f + 0.5f);

	float pressure; // in Pa
	pressureSensor.measurePressureOnce(pressure, oversampling);
	if(ret != 0)
	{
		DebugLog.print("Failed to measure pressure: "); DebugLog.println(ret);
		return;
	}

	struct PressureToAltitude
	{
		float pressure; // in Pa
		float altitude; // in meters above sea level
	};
	const PressureToAltitude altitudeTable[] =
	{
		{ 101325.0f, 0.0f },
		{ 100000.0f, 111.0f },
		{ 95000.0f, 540.0f },
		{ 90000.0f, 989.0f },
		{ 85000.0f, 1457.0f },
		{ 80000.0f, 1949.0f },
		{ 75000.0f, 2466.0f },
		{ 70000.0f, 3012.0f },
		{ 65000.0f, 3591.0f },
		{ 60000.0f, 4206.0f },
		{ 55000.0f, 4865.0f },
	};
	const int8_t altitudeTableNumValues = sizeof(altitudeTable)/sizeof(altitudeTable[0]);

	float alt = -std::numeric_limits<float>::max();
	for(int8_t i = 0; i < altitudeTableNumValues - 1; ++i)
	{
		if(i == altitudeTableNumValues - 2 || pressure >= altitudeTable[i+1].pressure)
		{
			const auto& p = altitudeTable[i];
			const auto& n = altitudeTable[i+1];
			alt = (pressure - p.pressure) / (n.pressure - p.pressure) * (n.altitude - p.altitude) + p.altitude;
			break;
		}
	}
	altitude = (int32_t)(alt * 1000.0f + 0.5f);

	/*DebugLog.print("temperature="); DebugLog.print(temp); DebugLog.print("°C");
	DebugLog.print("  pressure="); DebugLog.print(pressure); DebugLog.print("Pa");
	DebugLog.print("  altitude="); DebugLog.print(altitude); DebugLog.println("mm");*/
}

void loop()
{
	OTA.handle();
	handle_wifi_connection();
	handle_ADC_measures();
	handle_pressure_measure(); // also measures temperature

	unsigned long now = millis();
	static unsigned long lastLoopMillis = now;
	unsigned long dt = utils::elapsed(lastLoopMillis, now);
	lastLoopMillis = now;

	static DataLogger::Entry entry;
	entry.batteryVoltage = (float)batteryVoltage / 1000.0f;
	entry.batteryOutputCurrent = (float)batteryOutputCurrent / 1000.0f;
	entry.speed = getSpeed();
	entry.temperature = (float)temperature / 10.0f;
	entry.altitude = (float)altitude / 1000.0f;

	if(entry.speed > 0.0f)
	{
		stoppedSince = -1;
		if(!DataLogger::get().isOpen())
		{
			DebugLog.println("Starting DataLogger");
			DataLogger::get().open();
			tripDistance = 0;
			tripMovingTime = 0;
			tripTotalTime = 0;
			tripAscendingElevation = 0;
			tripMotorEnergy = 0;
		}
	}
	else
	{
		if(stoppedSince == -1)
		{
			stoppedSince = now;
		}
		else if(utils::elapsed(stoppedSince, now) > 5 * 60 * 1000)
		{
			if(DataLogger::get().isOpen())
			{
				DebugLog.println("Stopping DataLogger");
				DataLogger::get().close();
			}
		}
	}
	DataLogger::get().log(now, entry);

	bool isOnTrip = DataLogger::get().isOpen();
	bool isMoving = entry.speed > 0.0f;

	static unsigned long cumulatedMillis = 0;
	cumulatedMillis += dt;
	unsigned long newSeconds = cumulatedMillis / 1000;
	cumulatedMillis -= newSeconds * 1000;

	uint32_t currentMillimeters = speedSensorDistance;
	static uint32_t lastLoopMillimeters = currentMillimeters;
	uint32_t newMillimeters = utils::elapsed(lastLoopMillimeters, currentMillimeters);
	lastLoopMillimeters = currentMillimeters;

	static uint32_t cumulatedMillimeters = 0;
	cumulatedMillimeters += newMillimeters;
	uint32_t newMeters = cumulatedMillimeters / 1000;
	cumulatedMillimeters -= newMeters * 1000;

	uint32_t altitudeMillimeters = (uint32_t)(entry.altitude * 1000.0f + 0.5f);
	static uint32_t lastLoopAltitude = altitudeMillimeters;
	uint32_t altitudeChange = altitudeMillimeters - lastLoopAltitude;
	lastLoopAltitude = altitudeMillimeters;

	if(isOnTrip)
	{
		tripTotalTime += newSeconds;
		if(isMoving) tripMovingTime += newSeconds;
		tripDistance += newMeters;

		if(altitudeChange > 0)
			tripAscendingElevation += altitudeChange;

		uint32_t newEnergy = entry.batteryVoltage * entry.batteryOutputCurrent * ((float)dt / 1000.0f);
		tripMotorEnergy += newEnergy;
	}

	delay(isOnTrip ? 10 : 1000);
}