#ifndef PROPULSION_H
#define PROPULSION_H

#include "Debug.h"
#include "Constants.h"
//#include "Localization.h"
#include <Adafruit_MotorShield.h>

// Function Declarations
void move_V(int V_Trn);
void move_H(int V_Rot, int V_Trn);
void hover(float altitude);

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield(); 
Adafruit_DCMotor *motor_VL = AFMS.getMotor(2); 
Adafruit_DCMotor *motor_VR = AFMS.getMotor(1);
Adafruit_DCMotor *motor_HL = AFMS.getMotor(3);
Adafruit_DCMotor *motor_HR = AFMS.getMotor(4);

void Propulsion_PauseState(){
  move_V(0);
  move_H(0, 0);
}

void Propulsion_GoalFound(){
  move_V(Outputy);
  move_H(Outputx, BASE_SPEED);
}

void Propulsion_GoalSeeking(){
  #ifdef LOCALIZATION_H
    hover(altitudeCalc());
  #else
    move_V(0);
  #endif
  move_H(SEEKING_SPEED, 0);
}

void Propulsion_BallDetected(){
  move_V(Outputy);
  move_H(Outputx, BASE_SPEED);
}

void Propulsion_BallSeeking(){
  #ifdef LOCALIZATION_H
    hover(altitudeCalc());
  #else
    move_V(0);
  #endif
  move_H(SEEKING_SPEED, 0);
}

// V_Trn value should be between -255 to 255 with positive values moving the blimp upward.
void move_V(int V_Trn){
  if (V_Trn > 0) {
    // Move up
    panel.singleLED(DEBUG_VERTICALSPEED, abs(V_Trn), 0, 0);
    motor_VL->setSpeed(abs((int) V_Trn));
    motor_VR->setSpeed(abs((int) V_Trn)); 
    motor_VL->run(BACKWARD);
    motor_VR->run(BACKWARD);
  }
  else if(V_Trn < 0) {
    // Move down
    panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(V_Trn));
    motor_VL->setSpeed(abs((int) V_Trn));  
    motor_VR->setSpeed(abs((int) V_Trn)); 
    motor_VL->run(FORWARD);
    motor_VR->run(FORWARD);
  }
  else {
    // No motor action
    panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0);
    motor_VL->setSpeed(0);
    motor_VR->setSpeed(0);
  }
}

void move_H(int V_Rot, int V_Trn){
  int lspeed = -1*V_Rot + V_Trn;
  int rspeed =  1*V_Rot + V_Trn;

  if (lspeed > 0){
    motor_HL->run(FORWARD);
  }
  else {
    motor_HL->run(BACKWARD);
  }

  if (rspeed > 0){
    motor_HR->run(FORWARD);
  }
  else {
    motor_HR->run(BACKWARD);
  }
  
  displaySpeed(lspeed, rspeed);
  motor_HL->setSpeed(min(MAX_SPEED, abs(rspeed)));
  motor_HR->setSpeed(min(MAX_SPEED, abs(lspeed)));
}

#ifdef LOCALIZATION_H  
void hover(float altitude){
  float hoverAltitude = altitudeControl();
  move_V(BASE_SPEED*(hoverAltitude-altitudeCalc()));
}
#endif

#endif
