#include <Arduino_LSM6DS3.h>
#include <WiFiNINA.h>
#include "data.h"

#define SAMPLES_PER_MINUTE 256
#define SENSITIVITY 5
#define THRESHOLD 0.04
#define ID 1
#define DEBUG false

float accelX, accelY, accelZ;

float calX, calY, calZ;
unsigned char calibration;

float oneMinSamples[SAMPLES_PER_MINUTE];
unsigned char oneMinSampleIndex;
float oneMinMax;
int startTime, deltaTime;

void setup() 
{
	// Serial only used in debug mode
	if (DEBUG)
	{
		Serial.begin(9600);
		while (!Serial);
	}

	if (!IMU.begin())
	{
		if (DEBUG)
		{
			Serial.println("Failed to initialize IMU");
		}

		while(1);
	}
	
	// Setup LED
	pinMode(LEDR, OUTPUT);
	pinMode(LEDG, OUTPUT);
	pinMode(LEDB, OUTPUT);

	oneMinSampleIndex = 0;
	calibration = 0;
	calX = 0.0;
	calY = 0.0;
	calZ = 0.0;
}

void loop() 
{
	startTime = micros();
	// Calculate and update the square of the magnitude of acceleration
	if (IMU.accelerationAvailable())
	{
		IMU.readAcceleration(accelX, accelY, accelZ);
		accelX *= SENSITIVITY;
		accelY *= SENSITIVITY;
		accelZ *= SENSITIVITY;
	}

	
	// Calibration Stage
	if (calibration < 0xFF)
	{
		calibration++;
		if (DEBUG)
		{
			Serial.print("Calibrating (");
			Serial.print(calibration/2.55);
			Serial.print("%)\x0d");
		}
		digitalWrite(LEDR, LOW);
		digitalWrite(LEDG, LOW);
		digitalWrite(LEDB, HIGH);

		// Sum all accelerations over the calibration period
		calX += accelX;
		calY += accelY;
		calZ += accelZ;
		
		// Set the calibration values to the mean over the calibration period
		if (calibration == 0xFF)
		{
			calX /= calibration;
			calY /= calibration;
			calZ /= calibration;
		}
	}
	// Post-Calibration Stage
	else
	{
		// Adjust the result by the calibrated offset
		accelX -= calX;
		accelY -= calY;
		accelZ -= calZ;

		// Add the sample to the sample history
		oneMinSamples[oneMinSampleIndex] = sqrt(accelX*accelX + accelY*accelY + accelZ*accelZ);

		// Continuously calculate the maximum square of acceleration per minute
		oneMinMax = arrMaxf(oneMinSamples, SAMPLES_PER_MINUTE);
		if (DEBUG)
		{
			Serial.print("id:");
			Serial.print(ID);
			Serial.print(", equipmentStatus:");
			Serial.print((oneMinMax >= THRESHOLD) ? "Taken":"Free");
			Serial.print(", oneMinMax:");
			Serial.print(oneMinMax);
			Serial.print(", accelerationX:");
			Serial.print(accelX);
			Serial.print(", accelerationY:");
			Serial.print(accelY);
			Serial.print(", accelerationZ:");
			Serial.print(accelZ);
			Serial.print(", accelerationMag:");
			Serial.println(oneMinSamples[oneMinSampleIndex]);
		}
		if (oneMinMax >= THRESHOLD)
		{
			digitalWrite(LEDR, HIGH);
			digitalWrite(LEDG, LOW);
			digitalWrite(LEDB, LOW);
		}
		else
		{
			digitalWrite(LEDR, LOW);
			digitalWrite(LEDG, HIGH);
			digitalWrite(LEDB, LOW);
		}
	}

	// Calculate the time in microseconds the function took
	deltaTime = micros() - startTime;

	// 256 samples per minute
	delayMicroseconds(234375 - deltaTime);
	oneMinSampleIndex++;
}