#ifndef LOCALIZATION_H
#define LOCALIZATION_H

#include "Constants.h"
#include <Adafruit_MPL3115A2.h>

float groundLevel = 120;
float lowHeightBarrier  = 4;
float highHeightBarrier = 4;
float ceilingHeight = 20;

Adafruit_MPL3115A2 baro;

float hoverAltitude = -1;

// Function Declarations
float altitudeCalc();
float altitudeControl();

void Localization_Setup(){
  if (!baro.begin()) {
    Serial.println("Barometer not found");
    while(1);
  }
  baro.setSeaPressure(1016);
}

float Localization_Init(){
  return altitudeCalc();
}

void Localization_GoalFound(){
  altitudeControl();
}

void Localization_BallDetected(){
  hoverAltitude = -1;
}

float altitudeControl(){
  if (hoverAltitude == -1){
    hoverAltitude = altitudeCalc();
  }
  return hoverAltitude;
}

float altitudeCalc(){
  return baro.getAltitude() - groundLevel;
}

#endif
