/*--------------------------------------------------------------------------------
Filename: TOF.h
Description: Header file for adafruit VL_6180X TOF sensor
--------------------------------------------------------------------------------*/
#ifndef MOTOR_H
#define MOTOR_H

#include "mbed.h"
#include "Buzzer.h"
#include <ros.h>
#include <Servo.h>
#include <std_msgs/UInt8.h>

/*--------------------------------------------------------------------------------
---------------------------------DEFINES------------------------------------------
--------------------------------------------------------------------------------*/

//MOTOR 1
#define M1_IN1  PC_6
#define M1_IN2  PB_15
#define M1_PWM  PB_13
#define M1_A    PA_4
#define M1_B    PB_12

//MOTOR 2
#define M2_IN1  PA_15
#define M2_IN2  PC_7
#define M2_PWM  PB_4
#define M2_A    PB_5
#define M2_B    PB_3
#define srvoPan PE_5
#define srvoTilt PF_9


static Servo servo1(srvoTilt);
static Servo servo2(srvoPan);

/*--------------------------------------------------------------------------------
---------------------------------CLASSES------------------------------------------
--------------------------------------------------------------------------------*/

// Class for managing connection and state to a VL6180X sensor

class cMotor
{
public:
    cMotor(PwmOut pwm, DigitalOut fwd, DigitalOut rev);  //Constructor, initialised motor pinouts
    void Forwards(float speed); //Drives motor forwards
    void Backwards(float speed);//Drives motor backwards
    void Stop();    //Stops both motors
private:
    PwmOut _pwm;
    DigitalOut _fwd;
    DigitalOut _rev;

};

/*--------------------------------------------------------------------------------
-----------------------------------Functions---------------------------------------
--------------------------------------------------------------------------------*/
void Check_for_obstacles(uint8_t TOFRange[4]); //Obstacle avoidance functionality
void update_power_levels(float vBatt);  //Sends power level to the LED class for further processing
void MotorKeySub(const std_msgs::UInt8 &keyPress);
void servoKeySub(const std_msgs::UInt8 &ServoKeyPress);

/*--------------------------------------------------------------------------------
--------------------------------External Variables--------------------------------
--------------------------------------------------------------------------------*/
//extern cMotor MotorL;  //Left motor class and pin initialisation
//extern cMotor MotorR;  //Right motor class and pin initialisation


static int IncSize; //Increment size for servo movment in increments of 1 - 20 degrees
 

#endif