#include <Wire.h>
#include <math.h>
#include <inttypes.h>
//#include <Adafruit_MCP4725.h>
/*
  Mermaid's Rest - Helm Controls

  - Madison Kelly - [email protected]
  - Last Updated  - Jan. 27, 2024

  References and Sources
  - MCP4725 Tutorial - Electronoobs
    - https://www.youtube.com/watch?v=SgPbzAWIwlk

*/

// Remove this define in order to stop outputting to serial
#define DEBUG

//Adafruit_MCP4725 dac;
// Set this value to 9, 8, 7, 6 or 5 to adjust the resolution
#define DAC_RESOLUTION   (9)

// Forward declaration of init function
void initializeAverages();

// Switches
static const int32_t mainSwitchPin = 12; // When this switch is open, the motor signal will be set to 2.5v (idle)

// the setup function runs once when you press reset or power the board
void setup() {
  // Setup so we can read the serial port
#ifdef DEBUG
  Serial.begin(9600);
  Serial.println("MCP4725 Test");
#endif
  // Make sure to calculate correct sum for the very first sample
  pinMode(mainSwitchPin, INPUT_PULLUP);
  //dac.begin(0x62);
  // dac.setVoltage(dacValue, false); // Just to be on the safe side
  initializeAverages();
}

// Range for a 12-bit DAC
static const int32_t dacMinimum        = 0;
static const int32_t dacMaximum        = 4095;
static const int32_t dacVoltageMaximum = 5000;
static const int32_t dacVoltageMinimum = 0;

// How long to wait between loops (ms)
static const int32_t delayTime = 100;

// How many records to use when calculating our rolling average. This needs to
// be proportional to 'delayTime'
static const int32_t averageOver = 32;

// These might need to be tuned per helm control
static const int32_t dacNeutral      = 2010;    // Tested on this DAC, not mathmatically accurate
static const int32_t dacNeutralStart = 1965;
static const int32_t dacNeutralEnd   = 2130;

// -- Potentiometer values as read by digitalRead()
// Reverse starts at high pot and counts down
static const int32_t reverseMaximum = 265;
static const int32_t reverseMinimum = 465;

// Forward starts at low pot and counts up
static const int32_t forwardMinimum = 550;
static const int32_t forwardMaximum = 750;

// Reverse starts at high pot and counts down
static const int32_t reverseSensorSteps = (reverseMinimum - reverseMaximum);
static const int32_t forwardSensorSteps = (forwardMaximum - forwardMinimum);

// Calculate the nuetral range midpoint
static const int32_t neutralMidPoint = (reverseMinimum + forwardMinimum) / 2;

// This is for showing percentages.
static const int32_t reversePercentSteps = 100 / reverseSensorSteps;
static const int32_t forwardPercentSteps = 100 / forwardSensorSteps;

uint8_t oldSwitchVal = 1;
// This is used to know how many samples we have in our rolling average. It's
// main use is to deal with start-up when we don't yet have a full array.
int32_t sampleOver       = 0;
int32_t smoothedPotValue = 0;
int32_t oldestPotIndex   = 0;  // This is the array index with the oldest value
int32_t potTotal         = 0;
int32_t potArray[averageOver];

void initializeAverages() {
  for (int i = 0; i < averageOver; ++i) {
    potArray[i] = neutralMidPoint;
  }
  potTotal = averageOver * neutralMidPoint;
  oldestPotIndex = 0;
}

// the loop function runs over and over again forever
void loop() {
  // Read the control switch. If this is LOW, we set the motor control voltage to 2.5v / neutral
  int32_t mainSwitchVal = digitalRead(mainSwitchPin);

  // Read the helm position potentiometer if the main switch is on
  int32_t sensorValue;
  if (mainSwitchVal == 1) {
    if (oldSwitchVal != mainSwitchVal) {
      // We went from 0->1      
      initializeAverages();
    }

    // Switch is off, force the sensorValue to the middle of the neutral range
    sensorValue = neutralMidPoint;
  } else {
    // only read the sensor value if the main switch is on
    sensorValue = analogRead(A0);
    sensorValue = constrain(sensorValue, reverseMaximum, forwardMaximum);
  }
  
  oldSwitchVal = mainSwitchVal;
  // Smooth out the sensorValue over 'averageOver' samples.


  // We know that the potTotal is the sum of all values in the array
  // So the new sum is this sum minus the sample to be replaced plus the new sample.
  potTotal = potTotal + sensorValue - potArray[oldestPotIndex] ;


  // Replace oldes value
  potArray[oldestPotIndex] = sensorValue;
  oldestPotIndex = (oldestPotIndex + 1) % averageOver; // Modulo
  smoothedPotValue = potTotal / averageOver;

  //Serial.print("Oldest pot Index: [");
  //Serial.print(oldestPotIndex);
  //Serial.print("], pot total: [");
  //Serial.print(potTotal);
  //Serial.print("], smoothed sensor: [");
  //Serial.print(smoothedPotValue);
  //Serial.println("]");

  int32_t dacValue = dacNeutral;  // Default to 2.5v
  if (smoothedPotValue <= reverseMinimum)
  {
    // 465 = Min Reverse = DAC 1965 = 2358 mV
    // 265 = Max Reverse = DAC    0 -    0 mV
    dacValue = map(smoothedPotValue, reverseMaximum, reverseMinimum, dacVoltageMinimum, dacNeutralStart);
  }
  else if (smoothedPotValue >= forwardMinimum)
  {
    // 750 = Max Forward = DAC 4095 = 4914 mV
    // 550 = Min Forward = DAC 2130 = 2556 mV
    dacValue = map(smoothedPotValue, forwardMinimum, forwardMaximum, dacNeutralEnd, dacVoltageMaximum);
  }
  else
  {
    // In the Neutral deadzone, we want 2500v, DAC = 2084 (2500.8 mV)
    dacValue = dacNeutral;
  }

#ifdef DEBUG
  // -- Here we calculate ranges.
  int32_t IntegerDACValue = roundf(dacValue);
  int32_t IntegerVoltage  = (IntegerDACValue * 5000) / 4095;

  Serial.print("Switch: [");
  Serial.print(mainSwitchVal);
  Serial.print("], Raw Sensor: [");
  Serial.print(sensorValue);
  Serial.print("], Smoothed Sensor: [");
  Serial.print(smoothedPotValue);
  Serial.print("], DAC: [");
  Serial.print(IntegerDACValue);
  Serial.print("] (");
  Serial.print(IntegerVoltage);
  Serial.print(" mV)");

  if (smoothedPotValue <= reverseMaximum)
  {
    Serial.print(" - Reverse 100%");
  }
  else if (smoothedPotValue >= forwardMaximum)
  {
    Serial.print(" - Forward 100%");
  }
  else if ((smoothedPotValue >= reverseMaximum) && (smoothedPotValue <= reverseMinimum))
  {
    int32_t range      = reverseMinimum - reverseMaximum;
    int32_t position   = smoothedPotValue - reverseMinimum;
    int32_t percentage = -(int)roundf(((float)position / (float)range) * 100.0f);
    Serial.print(" - Reverse ");
    Serial.print(percentage);
    Serial.print("%");
  }
  else if ((smoothedPotValue >= forwardMinimum) && (smoothedPotValue <= forwardMaximum))
  {
    int32_t range      = forwardMaximum - forwardMinimum;
    int32_t position   = smoothedPotValue - forwardMinimum;
    int32_t percentage = (int)roundf(((float)position / (float)range) * 100.0f);
    Serial.print(" - Forward ");
    Serial.print(percentage);
    Serial.print("%");
  }
  else
  {
    Serial.print(" - Neutral");
  }

  Serial.println("");
#endif

  // Update the DAC
  dacValue = constrain(dacValue, dacMinimum, dacMaximum);
  //dac.setVoltage(dacValue, false);

  // Read 10/sec
  delay(delayTime);
}