/************************* quadCommand.h *********************************/
/*                                                                       */
/*************************************************************************/

#ifndef QUAD_COMMAND_H
#define QUAD_COMMAND_H

#include "mbed.h"
#include "motor.h"
#include "com.h"
#include "Sensor.h"
#include "PID.h"
#include "KL25Z_Watchdog.h"
#include "RGBled.h"


#define DOGTIMER 0.5f

// Motor constants.
#define MOTOR1              PTA5    // PTA5    // Pin used for motor 1.
#define MOTOR2              PTA4   // PTA4    // Pin used for motor 2. 
#define MOTOR3              PTA12    // PTA12   // Pin used for motor 3.
#define MOTOR4              PTD4    // PTD4    // Pin used for motor 4. 

// Xbee constants.
#define TXPIN               PTA2        // Pin used for xbee TX.
#define RXPIN               PTA1        // Pin used for xbee RX.
#define RSSIPIN             PTD5        // Pin used for xbee RSSI


// Sensor constants.
#define MMA8451_I2C_ADDRESS (0x1d<<1)   // Address of I2C accelerometer.
#define ACCSDA              PTE25       // Pin for accelerometer SDA line.
#define ACCSCL              PTE24       // Pin for accelerometer SCL line.

#define ROLLTHROTTLEGAIN    0
#define PITCHTHROTTLEGAIN   0
#define YAWTHROTTLEGAIN     4

// PID constants. 
// D = 450 shaking
//High throttle(<200) 0.6     0   450
//Low throttle(<100)  0.45    0.1 80
#define PID_P_PITCH 1.1                     // Proportional gain.
#define PID_I_PITCH 0.0                       // Integral gain.
#define PID_D_PITCH 140.0                       // Derivative gain.
#define PID_P_ROLL 1.1                       // Proportional gain.
#define PID_I_ROLL 0.0                     // Integral gain.
#define PID_D_ROLL 140.0                       // Derivative gain.
#define PID_P_YAW 2.2                    // Proportional gain.
#define PID_I_YAW 0.0                       // Integral gain.
#define PID_D_YAW 40.0                       // Derivative gain.
#define TRIM_VALUE 0.5f

#define THROTTLE_THRES 16

#define PITCH_RANGE 10
#define PITCH_MULTI 4
#define PITCH_LOW_RANGE -PITCH_RANGE*PITCH_MULTI+64
#define PITCH_HI_RANGE PITCH_RANGE*PITCH_MULTI+64

#define YAW_RANGE 60
#define YAW_MULTI 1
#define YAW_LOW_RANGE -YAW_RANGE*YAW_MULTI+64
#define YAW_HI_RANGE YAW_RANGE*YAW_MULTI+64

#define MAX_CORRECTION_VALUE 10.0       // Integral windup.
#define DEFAULT_WINDUP_GUARD 10         //MAX_CORRECTION_VALUE / PID_I_PITCH       // Integral windup.
#define YAW_WINDUP_GUARD 50

#define MOTOR_UPDATE    10             // Motor update speed.
#define TIMEOUT_TIMER   2.0

#define THROTTLEPERSEC  500.0 
#define THROTTLEJUMP THROTTLEPERSEC / 1000.0 * MOTOR_UPDATE
#define TIMEOUT  TIMEOUT_TIMER / MOTOR_UPDATE * 1000

#define ROLLYAWJUMP 2.0
#define YAWCUTOFF 10

class quadCommand
{
    public:
    quadCommand();  // Constructor
    void run();     // Main loop
    
    private:
    void rxInput();     // Receive data from Xbee
    void updateMotors();    // Send PID values to Motors
    void readyMotors();     // Callback function for update
    
    com *myCom; // Serial
    Sensor *mpu; // I2C
    motor *myMotor1; // 1 PWM
    motor *myMotor2; // 1 PWM
    motor *myMotor3; // 1 PWM
    motor *myMotor4; // 1 PWM
    PID pidPitch; 
    PID pidRoll;
    PID pidYaw;
    RGBled led; // 3 PWM
    Timer time; 
    Watchdog dog; // Timer
    Ticker motorProcess;
    
    volatile bool need2update;
         
    //Control Variables
    float targetThrottle,   previousThrottle,   throttleHelper,     throttle;
    float targetPitch,      followPitch,        currentPitch,       pitch;
    float targetRoll,       followRoll,         currentRoll,        roll;
//    float targetYaw,        followYaw,        currentYaw,         yaw;
    float targetGyroYaw,    followGyroYaw,    currentGyroYaw,     gyroYaw;
    
    float pitchTrim, rollTrim , gyroYawTrim;
    
    unsigned int rxTimeout;
    unsigned int dt;
    
};

#endif