/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Simple altitude logging code for first flight test. Used to gather data on system dynamics. Written by Euan French. //
// Future changes: - Include battery voltage measurement and compensation, somehow.                                    //
//                 - Include mass change compensation. (This and previous could be done by estimating hover throttle)  //
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <SD.h>
#include <SPI.h>
#include <EEPROM.h>
#include <Servo.h>
#include <PID_v1.h>



// Pins currently in use: 5 for throttle in, 6 for throttle out, 8 for echo pin, 9 for trigger pin, 3 for servo

// Global variables
Servo dropServo;
Servo naze32;
File logfile;
String logname; // This might not have to be global but it's no biggie
int servoPos;
bool autoOn;
unsigned long timer; // Timer variable (needs to be in startup and loop)
int unThrottleIn = 0;
int triggerPin = 9;
int echoPin = 8;
float alpha = 0.1;
float lastDistance = 0;
bool dropped = false;
double Setpoint, Input, Output;
double preKp=7.5, preKi=1, preKd=0.5; // Values before drop (PLACEHOLDER)
double postKp=7.5, postKi=1, postKd=0.5; // Values after drop (PLACEHOLDER)
PID myPID(&Input, &Output, &Setpoint, preKp, preKi, preKd, DIRECT);


// Custom functions

float ultrasonicRead() { // Uses delays, but they're tiny, so I think we can get away with it. Ripped from https://howtomechatronics.com/tutorials/arduino/ultrasonic-sensor-hc-sr04/
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  float duration = pulseIn(echoPin,HIGH);
  float distance_cm = ((duration+ 106.63)/57.609); // Calibrated with team test data
  float smoothedDistance = alpha*distance_cm + (1-alpha)*lastDistance;
  lastDistance = smoothedDistance;
//  if(fabs(smoothedDistance-distance_cm)>10){ // Code to make it more responsive to fast changes in altitude
//    return distance_cm;
//  }
//  else{
    return smoothedDistance;
//  }
}

// Standard functions

void setup()  {
  Serial.begin(9600);

  // Servo setup
  dropServo.attach(3);
  naze32.attach(6);

  // SD card setup
   pinMode(10, OUTPUT); // Using pin 10 for SD card but could change it according to https://learn.adafruit.com/adafruit-micro-sd-breakout-board-card-tutorial?view=all
   float lognum; // Increment variable, lets us make a new log for each flight, MAKE SURE YOU HAVE SET THIS AS 0 OR SOME OTHER FLOAT FIRST, USING ANOTHER SKETCH
   EEPROM.get(0, lognum); // EEPROM address for storing increment variable
   if(isnan(lognum)){EEPROM.put(0, 0);} // If the EEPROM has nothing in then start at 0
   int intLognum = round(lognum)+1; // Increment it
   EEPROM.put(0, float(intLognum));
   logname = "Log_"; // Name of the log files
   logname.concat(intLognum); // Add on the incremented float
   logname.concat(".csv"); // Make it a .csv
   Serial.println(logname);
   SD.begin(10);
   logfile = SD.open(logname, FILE_WRITE); // Create the log file and prepare it for writing to
   logfile.close();

 if (SD.exists(logname)) {
   Serial.println("log exists.");
 } else {
   Serial.println("log doesn't exist.");
 }

  // Ultrasonic setup
  pinMode(triggerPin, OUTPUT); // Sets the trigger pin, output
  pinMode(echoPin, INPUT); // Sets the echo pin, input


  // Accelerometer setup
  // This needs that whole FrSky telemetry decoding shit that i really dont want to write right now, so we'll just use the ultrasonic for now

  // Throttle logging setup
  pinMode(A0, INPUT);
  pinMode(5, INPUT);
  pinMode(4, OUTPUT);
  pinMode(6, OUTPUT);
  timer = millis(); // Start timer for logging
  dropServo.write(35);

  // PID Setup
  myPID.SetOutputLimits(1098, 1900);
  Setpoint = 100;

}

void loop() {
  Input = ultrasonicRead();

  if(pulseIn(A0, HIGH) < 1500){
    digitalWrite(4, LOW);
    myPID.SetMode(MANUAL);
    autoOn = false;
    unThrottleIn = (pulseIn(5, HIGH));
    //  Serial.println(unThrottleIn);
   }
   else if(pulseIn(A0,HIGH) > 1500 && autoOn==false){
    myPID.SetMode(AUTOMATIC);
    autoOn = true;
    digitalWrite(4, HIGH);
   }
    else{
      if(dropped==true)
      {
         myPID.SetTunings(postKp, postKi, postKd);
         myPID.Compute();
         naze32.write(Output);
         unThrottleIn = Output;
      }
      else
      {
         myPID.SetTunings(preKp, preKi, preKd);
         myPID.Compute();
         naze32.write(Output);
         unThrottleIn = Output;
      }
   }

  float currentAlt = ultrasonicRead();
  if(fabs(currentAlt-100) < 2 && dropped==false){
    dropServo.write(0);
    Serial.println("Opened!");
    logfile = SD.open(logname, FILE_WRITE);
    logfile.println("Servo opened!");
    logfile.close();
    delay(50);
    dropServo.write(50);
    dropped = true;
    };

  // Read Z accelerometer (assumption that angle is small)
  // Integrate twice
  // Sensor fuse, complimentary filter <- output estimate altitude

  // Write estimate altitude, throttle signal, and millis() to SD card
  unsigned long timeElapsed = (millis()-timer);
  String writeOut = "";
  writeOut.concat(timeElapsed);
  writeOut.concat(",");
  writeOut.concat(String(currentAlt));
  writeOut.concat(",");
  writeOut.concat(unThrottleIn);


  logfile = SD.open(logname, FILE_WRITE);
  logfile.println(writeOut);
  logfile.close();
  //Serial.println(writeOut);
  //Serial.println(currentAlt);
  Serial.println(unThrottleIn);
}