#include "Ultrasonic.h"
#include <Servo.h>
#include <Wire.h>
#include <Adafruit_BMP085.h>


Servo myThrottleChOut;

Adafruit_BMP085 bmp; //This is the barosensor. VCC to Ard3v, GND to ArdGnd,

// Connect VCC of the BMP085 sensor to 3.3V (NOT 5.0V!)
// Connect GND to Ground
// Connect SCL to i2c clock - on '168/'328 Arduino Uno/Duemilanove/etc thats Analog 5
// Connect SDA to i2c data - on '168/'328 Arduino Uno/Duemilanove/etc thats Analog 4
// EOC is not used, it signifies an end of conversion
// XCLR is a reset pin, also not used here

int throttlePin1 = 11;
int pinReadCh6 = 6; //Reads channel 6 from RX
int pinReadCh3 = 3; //Reads channel 3 from RX

unsigned long duration1;
unsigned long duration2;
unsigned long duration3; //duration to use inside the altitude hold loop

Ultrasonic ultrasonic(12,13); //12 = digitalpin 12 -> trig; 13 = digitalpin 13 -> echo;


//Future implementation:
//If the ch6 is turned to max arduino enters altitude control
//while in the altitude control - read ch3 (throttle).
//if ch3 > 50% hold the altitude
//if ch3 < 50% decrease throttle just below hoover level to make tricopter land slowly.

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

bmp.begin();

pinMode(pinReadCh3, INPUT);
pinMode(pinReadCh6, INPUT);
myThrottleChOut.attach(throttlePin1); //attach the "servo" to digital pin 7, this is the digital pin which is going to write to the KKboards throttle channel.

}

void loop()
{

  duration1 = pulseIn(pinReadCh3, HIGH); //Reads the throttle level, returns 1150 - 1839 microseconds. 1 millisecond = 1000 microseconds -> duration1/1000: ex 1,736ms
  duration2 = pulseIn(pinReadCh6, HIGH); //Reads the ch6 level

  //Serial.print(bmp.readPressure());


  if (duration2 > 1700) //If the ch6 knob is turned almost to max, arduino enter altitude hold mode
  {
    int distance = (ultrasonic.Ranging(CM)); //Gets the altitude when entering altitude hold mode, this will be a reference                                             //Serial.print(bmp.readPressure());
    while (duration2 > 1700) //If the ch6 knob is turned almost to max, arduino enters altitude control loop.
    duration3 = duration1;
    {
      int newDistance = (ultrasonic.Ranging(CM)); //Gets the current altitude, this will be compared to the reference altitude                                          //Serial.print(bmp.readPressure());
      if (newDistance > distance + 15) //+ 10 will give an intervall between desired altitude +- 10 cm in the upper and lower bounds.
      {
        //TODO: implement algorithm to calculate how much to decrease on throttle depending
        // on how far away the newDistance is from the desired reference altitude (distance).
        //close to reference alt: small increments to throttle
        //far away from reference alt: big increments to throttle
        if(duration3 > 1550) //this is to prevent the tricopter to throttling down to no power and fall from the sky
        {
        duration3 -= 5;
        }
        myThrottleChOut.writeMicroseconds(duration3);
      }
      else if(newDistance < distance - 15) // - 10 will give an intervall between desired altitude +- 10 cm in the upper and lower bounds.
      {
        if(duration3 < 1680) //this is to prevent the tricopter from throttling up to max throttle
        {
        duration3 += 5;
        }
        myThrottleChOut.writeMicroseconds(duration3);
      }
      delay(20);
      duration2 = pulseIn(pinReadCh6, HIGH); //Reads the ch6 level, this is needed to be able to exit the while-iteration
    }


  }
  else
    {
       myThrottleChOut.writeMicroseconds(duration1);
    }






  delay(20);
}