#include <EEPROM.h>

// start reading from the first byte (address 0) of the EEPROM
int address = 0;
byte value;

// Joystick vars
const int analogInPin = A0;

int sensorValue = 0;
int outputValue = 0;

//volatile int center[6] = {0, 0, 0, 0, 0, 0};
volatile int minvalue[6] = {1000, 1000, 1000, 1000, 1000, 1000};
volatile int maxvalue[6] = {0, 0, 0, 0, 0, 0};

void JoystickInit()
{
  // Initialize joysticks first
  value = EEPROM.read(address);

  if (value != 0)
  {
    address++;
    for (int i = 0; i < 6; i++)
    {
      address += sizeof(int);
      EEPROM.get(address, minvalue[i]);
      address += sizeof(int);
      EEPROM.get(address, maxvalue[i]);
      //      Serial.println(minvalue[i]);
      //      Serial.println(maxvalue[i]);
    }
  }
  else
  {
    // if eeprom empty
    delay(500);

    //    for (int i = 0; i < 6; i++)
    //    {
    //      center[i] = analogRead(analogInPin + i);
    //    }

    int time = 10000;
    while (millis() < time)
    {
      for (int i = 0; i < 6; i++)
      {
        sensorValue = analogRead(analogInPin + i);
        if (sensorValue > maxvalue[i]) {
          maxvalue[i] = sensorValue;
        }
        if (sensorValue < minvalue[i]) {
          minvalue[i] = sensorValue;
        }
      }
    }

    //  for (int i = 0; i < 6; i++)
    //  {
    //    minvalue[i] = center[i] - minvalue[i];
    //    maxvalue[i] = (maxvalue[i] - center[i]);
    //  }

    // write callibration to eeprom
    address = 0;
    EEPROM.write(address, 1);

    address++;
    for (int i = 0; i < 6; i++)
    {
      address += sizeof(int);
      EEPROM.put(address, minvalue[i]);
      address += sizeof(int);
      EEPROM.put(address, maxvalue[i]);
      //      Serial.println(minvalue[i]);
      //      Serial.println(maxvalue[i]);
    }
  }
}

struct joyStickdataStruct {
  float value[6];
} myJoyStickdataStruct;

joyStickdataStruct JoystickGet(bool debug = false)
{
  for (int i = 0; i < 6; i++)
  {
    sensorValue = analogRead(analogInPin + i);

    if (sensorValue > maxvalue[i])
      maxvalue[i] = sensorValue;
    if (sensorValue < minvalue[i])
      minvalue[i] = sensorValue;

    outputValue = map(sensorValue, minvalue[i], maxvalue[i], 0, 255);
    outputValue = sensorValue > maxvalue[i] ? 255 : sensorValue < minvalue[i] ? 0 : outputValue;
    //    if (sensorValue <= center[i])
    //    {
    //      outputValue = map(sensorValue, minvalue[i], center[i], 0, 127);
    //    }
    //    else
    //    {
    //      outputValue = map(sensorValue, center[i], maxvalue[i], 128, 255);
    //    }

    myJoyStickdataStruct.value[i] = outputValue;

    if (debug)
    {
      Serial.print(i);
      Serial.print("_");
      Serial.print(myJoyStickdataStruct.value[i]);
      if (i != 5)
        Serial.print(",");
      //    Serial.print(" ");
    }
  }
  if (debug)
    Serial.println();

  return myJoyStickdataStruct;
}