#include <Wire.h> //I2C library
#define ad_ROM_1_I2C 0x50
#include "SparkFunMPL3115A2.h"
#include <Servo.h>

// Constantes
const int A_lim = 10;
const float marge = 0.5;
const int decoupleur = 5;
const int lock = 3;
const int maintenance = 4;
const int buzzer = 2;
const int led = 13;

// Constantes à fixer
const int ouverture = 0;//position du servo découpleur ouvert
const int fermeture = 25;//position du servo découpleur fermé

// Variables validées
int n; //compteur de boucle
bool en_vol = true; //indique la fin de vol
bool recovery = false; //indique si le systeme de recup a été activé
bool locked; //true = armé et prêt au lancement, false si réglage
bool separateur_ouvert; // état du séparateur
float A_new, A_old, A_ini, t, calcul;
unsigned int AdrBaseEeprom =0; //adresse de l'EEPROM démarre à 0
unsigned int nb_data; //nombre de valeurs lues

//capteur de pression
MPL3115A2 myPressure;

//Servo de découplage
Servo myServo;
  
void setup() 
{
  Wire.begin();  // Init port I2C
  Serial.begin(9600); // Init port série
  
  myServo.attach(decoupleur); // attache servo sur E/S
  myServo.write(fermeture);//position ouverte par defaut
  myPressure.begin(); // capteur de pression actif

  pinMode(lock, INPUT);// pour la tirette de sécurité
  pinMode(maintenance, INPUT);// pour la manipulaton du separateur
  pinMode(led, OUTPUT);// voyant led de controle de la carte Nano
  
  if (digitalRead(lock) == HIGH) {
    locked = false;//tirette enclenchée, interrupteur fermé, pin à +5V
  }
  else {
    locked = true;//tirette retirée, interrupteur fermé, pin à 0
  }
  
  //Configure the sensor
  myPressure.setModeAltimeter(); // Measure altitude above sea level in meters
  myPressure.setOversampleRate(6); // Set Oversample to 64 means 258ms between two measures
  myPressure.enableEventFlags(); // Enable all three pressure and temp event flag
  //
  nb_data = 0;
  A_ini = 0;
  //Prise de l'altitude initiale
  for(int i=0; i<5; i++){
    A_ini = A_ini + myPressure.readAltitude();
  }
  A_ini = A_ini / 5;
  A_old = A_ini;
  Serial.println("Init altitude intiale");
  Serial.println(A_ini);
  //A_ini est la moyenne de 5 mesures

  //code d'essai
  locked = false;
  n=0;
}

void i2c_eeprom_write_byte(unsigned int eeaddress, byte data ) 
{
  Wire.beginTransmission(ad_ROM_1_I2C);
  Wire.write((int)(eeaddress >> 8)); // envoi du MSB de l'adresse
  Wire.write((int)(eeaddress & 0xFF)); // envoi du LSB de l'adresse
  Wire.write(data); //écriture de l'octet
  Wire.endTransmission();
  delay(5);// les écritures consécutives trop rapide mettent leur fiabilité en vrac (marche a partir de 3ms)    
}

void ecrire_reel(float var)
{
  //écriture d'un réel dans la mémoire
    byte *ptr;
    ptr = (byte *)&var;
    for(byte j=0;j<4;j++)
    {
      byte octet = *(ptr+j);
      i2c_eeprom_write_byte(AdrBaseEeprom++,octet);
    }    
}

byte i2c_eeprom_read_byte(unsigned int eeaddress )
{
  // routine de lecture de byte individuel dans l'EEPROM
  // lui passer adresse, elle rend le byte correspondant
  byte rdata = 0xFF;
  Wire.beginTransmission(ad_ROM_1_I2C);
  Wire.write((int)(eeaddress >> 8)); // envoi du MSB de l'adresse
  Wire.write((int)(eeaddress & 0xFF)); // envoi du LSB de l'adresse
  Wire.endTransmission();
  Wire.requestFrom(ad_ROM_1_I2C,1);
  if (Wire.available()) rdata = Wire.read(); //lecture de l'octet adressé
  return rdata;
}

void loop() 
{
  //mode de vol
  while ((en_vol == true)&(locked == true)){//la boucle s'arrête si on est retombé ou que la tirette de sécu a été mise
    digitalWrite(led, HIGH);//led allumée fixe
    if (n == 0){
      Serial.println("mode vol");
      n += 1;
    }
    A_new = myPressure.readAltitude() - A_ini;
    t = millis();
    ecrire_reel(t);
    nb_data++;
    ecrire_reel(A_new);
    // pour debugage
    //Serial.println(A_new);
    //
    nb_data++;
    if ((A_new<A_old)&(A_new>A_lim)){//detection de l'apogée après le franchissement de l'altitude limite
      //commande de découplage 
      myServo.write(ouverture);
      recovery = true;
      ecrire_reel(t);
      ecrire_reel(0.0); //indique l'ouverture du servo dans l'enregistrement des données
    }
    if ((A_new<A_old+marge)&(A_new>A_old-marge)&(recovery==true)){//cas ou l'altitude ne varie plus beaucoup après l'éjection du parachute
      en_vol = false;
      ecrire_reel(t);
      ecrire_reel(0.0); //indique l'impact au sol dans l'enregistrement des données
    }
    A_old = A_new;
    if (digitalRead(lock) == HIGH){ // verif de l'état de la tirette de sécu
      locked = false;
    }
    else {
      locked = true;
    }
    if (locked == false){
      n = 0;
    }
  }
  
  //Faire sonner le buzzer au sol
  if (en_vol == false){
    Serial.println("signal sonore");
    tone(buzzer, 440);
    delay(1000);
    noTone(buzzer);
    delay(500);
    
  }
 if (digitalRead(lock)== HIGH){
    locked = false;
 }
 else {
    locked = true;
 }
  
  //mode maintenance - manipulation du separateur
  while (locked == false){
    digitalWrite(led, LOW);//led eteinte
    if (n == 0){
      Serial.println("mode maintenance");
      n += 1;
    }
    if (digitalRead(maintenance) == HIGH){
      if (separateur_ouvert == false){
        Serial.println("decoupleur ouvert");
        myServo.write(ouverture);
        separateur_ouvert = true;
        delay(300);
      }
      else{
        Serial.println("decoupleur fermé");
        myServo.write(fermeture);
        myServo.write(fermeture-3);
        myServo.write(fermeture);
        myServo.write(fermeture-2);
        myServo.write(fermeture);
        separateur_ouvert = false;
        delay(300);
      }
    }
    delay(150);
    digitalWrite(led, HIGH);//led allumée
    delay(150);
    if (digitalRead(lock)== HIGH){
      locked = false;
    }
    else {
      locked = true;
    }
    if (locked == true){
      n = 0;
    }
  }
}