#include <Wire.h>
#include <MPU6050.h>
#include <stdio.h>
#include <math.h>
#include <vector>

const int16_t MIN_RAW_ACC = -32768;
const int16_t MAX_RAW_ACC = 32767;
const int16_t MIN_ACC = -2;
const int16_t MAX_ACC = 2;

const int16_t DATA_COLLECT_INTERVAL = 200;
const int16_t DATA_PRINT_INTERVAL = 5000;

MPU6050 accelerometer;

float remap(float value, float fromLow, float fromHigh, float toLow, float toHigh)
{
  float normalizedValue = (value - fromLow) / (fromHigh - fromLow);
  float remappedValue = normalizedValue * (toHigh - toLow) + toLow;
  return remappedValue;
}

class Vector3f
{
public:
  float x; 
  float y; 
  float z; 

  Vector3f(); 
  Vector3f(float x, float y, float z);
  Vector3f GetNormalizedAcceleration(); 
  Vector3f GetAcceleration(); 
  void PrintVector(); 
};


class AllData
{
public: 
  std::vector<Vector3f> allVectors; 

  AllData();
  std::vector<Vector3f>* GetAllVectors();
  Vector3f calculateAverage(); 
  Vector3f calculateVariance(); 
  Vector3f calculateStandardDeviation();
  Vector3f calculateMaxValues(); 
  Vector3f calculateMinValues(); 
  Vector3f calculateMinMaxDifference(); 
  Vector3f calculateMedian();

  void PrintStatistics();
};

AllData::AllData()
{
  allVectors = {};
}

std::vector<Vector3f>* AllData::GetAllVectors()
{
  return &allVectors;
}

Vector3f AllData::calculateAverage()
{
  Vector3f average = Vector3f(0.0f, 0.0f, 0.0f);
  int dataSize = allVectors.size();

  for (int i = 0; i < dataSize; i++)
  {
    average.x += allVectors[i].x;
    average.y += allVectors[i].y;
    average.z += allVectors[i].z;
  }

  average.x /= dataSize;
  average.y /= dataSize;
  average.z /= dataSize;

  return average;
} 

Vector3f AllData::calculateVariance()
{
  Vector3f average = this->calculateAverage();
  Vector3f variance = {0.0f, 0.0f, 0.0f};
  int dataSize = allVectors.size();

  for (int i = 0; i < dataSize; i++)
  {
    variance.x += pow(allVectors[i].x - average.x, 2);
    variance.y += pow(allVectors[i].y - average.y, 2);
    variance.z += pow(allVectors[i].z - average.z, 2);
  }

  variance.x /= dataSize;
  variance.y /= dataSize;
  variance.z /= dataSize;

  return variance;
} 

Vector3f AllData::calculateStandardDeviation()
{
  Vector3f variance = calculateVariance();
  Vector3f standardDeviation = {0.0f, 0.0f, 0.0f};

  standardDeviation.x = sqrt(variance.x);
  standardDeviation.y = sqrt(variance.y);
  standardDeviation.z = sqrt(variance.z);

  return standardDeviation;
} 

Vector3f AllData::calculateMaxValues()
{
  Vector3f maxValues = {allVectors[0].x, allVectors[0].y, allVectors[0].z};
  int dataSize = allVectors.size();

  for (int i = 0; i < dataSize; i++)
  {
    if (allVectors[i].x > maxValues.x)
    {
      maxValues.x = allVectors[i].x;
    }
    if (allVectors[i].y > maxValues.y)
    {
      maxValues.y = allVectors[i].y;
    }
    if (allVectors[i].z > maxValues.z)
    {
      maxValues.z = allVectors[i].z;
    }
  }

  return maxValues;
} 

Vector3f AllData::calculateMinValues()
{
  Vector3f minValues = {allVectors[0].x, allVectors[0].y, allVectors[0].z};
  int dataSize = allVectors.size();

  for (int i = 0; i < dataSize; i++)
  {
    if (allVectors[i].x < minValues.x)
    {
      minValues.x = allVectors[i].x;
    }
    if (allVectors[i].y < minValues.y)
    {
      minValues.y = allVectors[i].y;
    }
    if (allVectors[i].z < minValues.z)
    {
      minValues.z = allVectors[i].z;
    }
  }

  return minValues;
} 

Vector3f AllData::calculateMinMaxDifference()
{
  Vector3f minValues = calculateMinValues();
  Vector3f maxValues = calculateMaxValues();

  Vector3f difference;
  difference.x = maxValues.x - minValues.x;
  difference.y = maxValues.y - minValues.y;
  difference.z = maxValues.z - minValues.z;

  return difference;
}

Vector3f AllData::calculateMedian()
{
  int dataSize = allVectors.size();

  float xValues[dataSize];
  float yValues[dataSize];
  float zValues[dataSize];

  for (int i = 0; i < dataSize; i++)
  {
    xValues[i] = allVectors[i].x;
    yValues[i] = allVectors[i].y;
    zValues[i] = allVectors[i].z;
  }
  for (int i = 0; i < dataSize; i++)
  {
    for (int j = i + 1; j < dataSize; j++)
    {
      if (xValues[j] < xValues[i])
      {
        float temp = xValues[i];
        xValues[i] = xValues[j];
        xValues[j] = temp;
      }
      if (yValues[j] < yValues[i])
      {
        float temp = yValues[i];
        yValues[i] = yValues[j];
        yValues[j] = temp;
      }
      if (zValues[j] < zValues[i])
      {
        float temp = zValues[i];
        zValues[i] = zValues[j];
        zValues[j] = temp;
      }
    }
  }

  Vector3f median;
  int midIndex = dataSize / 2;

  median.x = (dataSize % 2 == 0) ? (xValues[midIndex - 1] + xValues[midIndex]) / 2.0f : xValues[midIndex];
  median.y = (dataSize % 2 == 0) ? (yValues[midIndex - 1] + yValues[midIndex]) / 2.0f : yValues[midIndex];
  median.z = (dataSize % 2 == 0) ? (zValues[midIndex - 1] + zValues[midIndex]) / 2.0f : zValues[midIndex];

  return median;
}

void AllData::PrintStatistics()
{
  Serial.print("Average : ");
  calculateAverage().PrintVector();
  Serial.print("Variance : ");
  calculateVariance().PrintVector();
  Serial.print("Standard Deviation : ");
  calculateStandardDeviation().PrintVector();
  Serial.print("Max : ");
  calculateMaxValues().PrintVector();
  Serial.print("Min : ");
  calculateMinValues().PrintVector();
  Serial.print("Difference : ");
  calculateMinMaxDifference().PrintVector();
  Serial.print("Median : ");
  calculateMedian().PrintVector();

  Serial.print("Collected records num:");
  Serial.println(allVectors.size());

  Serial.println();
}

Vector3f::Vector3f(float x, float y, float z)
{
  this->x = x; 
  this->y = y; 
  this->z = z;
}

Vector3f::Vector3f()
{
  this->x = 0.0f; 
  this->y = 0.0f; 
  this->z = 0.0f;
}

Vector3f Vector3f::GetNormalizedAcceleration()
{
  this->x = remap(this->x, MIN_RAW_ACC, MAX_RAW_ACC, MIN_ACC, MAX_ACC);
  this->y = remap(this->y, MIN_RAW_ACC, MAX_RAW_ACC, MIN_ACC, MAX_ACC);
  this->z = remap(this->z, MIN_RAW_ACC, MAX_RAW_ACC, MIN_ACC, MAX_ACC);
  return *this;
}

Vector3f Vector3f::GetAcceleration()
{
  int16_t x_copy, y_copy, z_copy;
  accelerometer.getAcceleration(&x_copy, &y_copy, &z_copy);

  x = (float) x_copy; 
  y = (float) y_copy; 
  z = (float) z_copy;

  return *this;
}

void Vector3f::PrintVector()
{
  Serial.print("x: ");
  Serial.print(this->x);
  Serial.print(", y: ");
  Serial.print(this->y);
  Serial.print(", z: ");
  Serial.println(this->z);
}

AllData collectedData;

void setup()
{
  Serial.begin(9600);
  Wire.begin();
  accelerometer.initialize();
}

void loop()
{
  Vector3f new_vector;  

  unsigned long startMillis = millis();
  unsigned long printMillis = startMillis + DATA_PRINT_INTERVAL;

  while (millis() < printMillis)
  {
    unsigned long currentMillis = millis();
    if (currentMillis - startMillis >= DATA_COLLECT_INTERVAL)
    {
      new_vector = new_vector.GetAcceleration();
      collectedData.GetAllVectors()->push_back(new_vector.GetNormalizedAcceleration());
      startMillis = currentMillis;
    }
  }

  collectedData.PrintStatistics();
}