#include "mbed.h"
#include "rtos.h"
#include "PID.h"
#include "QEI.h"

//**********************************************
//*            Constants Definitions           *
//**********************************************
//Photointerrupter input pins
#define I1pin D2
#define I2pin D11
#define I3pin D12

//Incremental encoder input pins
#define CHA D7
#define CHB D8

//Motor Drive output pins   //Mask in output byte
#define L1Lpin D4  //0x01
#define L1Hpin D5  //0x02
#define L2Lpin D3  //0x04
#define L2Hpin D6  //0x08
#define L3Lpin D9  //0x10
#define L3Hpin D10 //0x20

DigitalIn I1(I1pin);
DigitalIn I2(I2pin);
DigitalIn I3(I3pin);

DigitalIn CHAInput(CHA);
DigitalIn CHBInput(CHB);

//**********************************************
//*             New Constants                  *
//**********************************************

//Motor Drive outputs
DigitalOut *L1Ldigi = new DigitalOut(L1Lpin);
DigitalOut *L1Hdigi = new DigitalOut(L1Hpin);
DigitalOut *L2Ldigi = new DigitalOut(L2Lpin);
DigitalOut *L2Hdigi = new DigitalOut(L2Hpin);
DigitalOut *L3Ldigi = new DigitalOut(L3Lpin);
DigitalOut *L3Hdigi = new DigitalOut(L3Hpin);

PwmOut *L1Lpwm = new PwmOut(L1Lpin);
PwmOut *L1Hpwm = new PwmOut(L1Hpin);
PwmOut *L2Lpwm = new PwmOut(L2Lpin);
PwmOut *L2Hpwm = new PwmOut(L2Hpin);
PwmOut *L3Lpwm = new PwmOut(L3Lpin);
PwmOut *L3Hpwm = new PwmOut(L3Hpin);

DigitalOut led1(LED1);
RawSerial pc(SERIAL_TX, SERIAL_RX);
QEI wheel(CHA, CHB, NC, 117);

int8_t intState = 0;
int8_t intStateOld = 0;
int8_t orState = 0;

//Drive state to output table | Mapping from interrupter inputs to sequential rotor states. 0x00 and 0x07 are not valid
const int8_t driveTable[] = {0x12, 0x18, 0x09, 0x21, 0x24, 0x06, 0x00, 0x00};
const int8_t stateMap[] = {0x07, 0x05, 0x03, 0x04, 0x01, 0x00, 0x02, 0x07};    // Used for +ve velocity (CW)
//const int8_t stateMap2[] = {0x07, 0x01, 0x03, 0x02, 0x05, 0x00, 0x04, 0x07};    // Used for -ve velocity (ACW)
const int8_t lead = 1;                                                         //2 for forwards, -2 for backwards

const float Vref = 20;
const float Rref = 1000;
const float thresholdRPS = 38;              //Used to toggle between PWM and DigitalOut

const float RPS_SAMPLING_RATE = 0.1;      // rate at which interrupt is called, also the time difference(t) since the last interrupt call
float lastPosition = 0;
float currentPosition = 0;
float currentRPSValue = 0;               // global angular velocity/RPS value

float targetPosition=117*Rref;
float PIDrate = 0.2;
float Kc = 5.0;
float Ti = 0.0;
float Td = 0.0;
bool AUTO = 1;
float dutyCycle = 0;                    //global duty cycle to be passed to motorOut
float oriDutyCycle = 0;
float tmpdutycycle = 0;
PID controller(Kc, Ti, Td, PIDrate);
Thread PIDthread;

Ticker sampleRPS;
Ticker PrintRPS;
Ticker PrintKpara;

//**********************************************
//*             Function Definitions           *
//**********************************************
//------- Convert photointerrupter inputs to a rotor state -------
inline int8_t readRotorState()
{
    return stateMap[I1 + 2 * I2 + 4 * I3];
}

//------- Modulus Function -------
float modulus(float n) {
    if(n<0) {
        n=-n;
        return n;
    } else {
        return n;
    }
}

//------- Run the motor with speed limitation -------
void motorOut(int8_t driveState){

    int8_t driveOut = driveTable[driveState & 0x07];

    if (currentRPSValue <= thresholdRPS) {      //Use PWM at low speeds
        if(L1Lpwm == NULL){                 // if PWM pointers point to nothing, create new PwmOut Pins
            L1Lpwm = new PwmOut(L1Lpin);
            L1Hpwm = new PwmOut(L1Hpin);
            L2Lpwm = new PwmOut(L2Lpin);
            L2Hpwm = new PwmOut(L2Hpin);
            L3Lpwm = new PwmOut(L3Lpin);
            L3Hpwm = new PwmOut(L3Hpin);
        }

        if(L1Ldigi != NULL){            // if digital pin pointers point to something, delete them
            delete L1Ldigi;
            delete L1Hdigi;
            delete L2Ldigi;
            delete L2Hdigi;
            delete L3Ldigi;
            delete L3Hdigi;
            L1Ldigi = NULL;
            L1Hdigi = NULL;
            L2Ldigi = NULL;
            L2Hdigi = NULL;
            L3Ldigi = NULL;
            L3Hdigi = NULL;
        }

        if (~driveOut & 0x01) *L1Lpwm = 0;
        if (~driveOut & 0x02) *L1Hpwm = 1;
        if (~driveOut & 0x04) *L2Lpwm = 0;
        if (~driveOut & 0x08) *L2Hpwm = 1;
        if (~driveOut & 0x10) *L3Lpwm = 0;
        if (~driveOut & 0x20) *L3Hpwm = 1;

        if (currentRPSValue < Vref) {
            if (driveOut & 0x01) L1Lpwm->write(dutyCycle);
            if (driveOut & 0x02) L1Hpwm->write(1-dutyCycle);
            if (driveOut & 0x04) L2Lpwm->write(dutyCycle);
            if (driveOut & 0x08) L2Hpwm->write(1-dutyCycle);
            if (driveOut & 0x10) L3Lpwm->write(dutyCycle);
            if (driveOut & 0x20) L3Hpwm->write(1-dutyCycle);
        }
    }

    else { //High Speed use digitalpin
        if(L1Ldigi == NULL){
            L1Ldigi = new DigitalOut(L1Lpin);
            L1Hdigi = new DigitalOut(L1Hpin);
            L2Ldigi = new DigitalOut(L2Lpin);
            L2Hdigi = new DigitalOut(L2Hpin);
            L3Ldigi = new DigitalOut(L3Lpin);
            L3Hdigi = new DigitalOut(L3Hpin);
        }
        if(L1Lpwm != NULL){
            delete L1Lpwm;
            delete L1Hpwm;
            delete L2Lpwm;
            delete L2Hpwm;
            delete L3Lpwm;
            delete L3Hpwm;
            L1Lpwm = NULL;
            L1Hpwm = NULL;
            L2Lpwm = NULL;
            L2Hpwm = NULL;
            L3Lpwm = NULL;
            L3Hpwm = NULL;
        }

        if (~driveOut & 0x01) *L1Ldigi = 0;
        if (~driveOut & 0x02) *L1Hdigi = 1;
        if (~driveOut & 0x04) *L2Ldigi = 0;
        if (~driveOut & 0x08) *L1Hdigi = 1;
        if (~driveOut & 0x10) *L3Ldigi = 0;
        if (~driveOut & 0x20) *L1Hdigi = 1;

        if(currentRPSValue < Vref) {
            if (driveOut & 0x01) *L1Ldigi = 1;
            if (driveOut & 0x02) *L1Hdigi = 0;
            if (driveOut & 0x04) *L2Ldigi = 1;
            if (driveOut & 0x08) *L2Hdigi = 0;
            if (driveOut & 0x10) *L3Ldigi = 1;
            if (driveOut & 0x20) *L3Hdigi = 0;
        }
    }
}

//------- Basic synchronisation routine -------
int8_t motorHome()
{
    motorOut(0);
    wait(1.0);
    return readRotorState();
}

//------- Map duty cycle -------
void dutyMap() {
  if(oriDutyCycle<0) {
          tmpdutycycle = oriDutyCycle;
          tmpdutycycle=-tmpdutycycle;
      }
  else {
          tmpdutycycle = oriDutyCycle;
       }
    dutyCycle = tmpdutycycle*0.275 + 0.725;

}

//------- Drive motor using position sensed by PI -------
//+ve lead = forward | -ve lead = acw
inline void readPIrunMotor(){
    // pc.printf("QEI rps: %f \t", currentRPSValue);
    // pc.printf("QEI count: %f \n\r", currentPosition);
    intState = readRotorState();
    if (intState != intStateOld) {
        intStateOld = intState;
        if (oriDutyCycle > 0) {
            dutyMap();
            motorOut((intState - orState + lead + 6 ) % 6); //Forwards
        }
        if (oriDutyCycle < 0) {
            dutyMap();
            motorOut((intState - orState - lead + 6 ) % 6); //backwards
        }
    }
}

//------- Speed from QEI and PI -------
void getRPSfromQEI(){
    lastPosition = currentPosition;
    currentPosition = wheel.getPulses();    //getPulses gets accumulated no. of pulses recorded
    float numberOfRevolutions = (currentPosition - lastPosition) / 117;
    currentRPSValue = modulus((numberOfRevolutions / RPS_SAMPLING_RATE));
}

//------- Controller -------
void controlInit() {
    controller.setInputLimits(0.0,targetPosition);
    controller.setOutputLimits(-1.0, 1.0);
    controller.setBias(0.0);
    controller.setMode(AUTO);
    controller.setSetPoint(targetPosition);
    // pc.printf("Kc: %f\n",controller.getPParam());
    // pc.printf("Ki: %f\n",controller.getIParam());
    // pc.printf("Kd: %f\n",controller.getDParam());
}

void controlR() {
    while(1) {
        controller.setProcessValue(modulus(currentPosition));
        oriDutyCycle = controller.compute();
        Thread::wait(PIDrate);
    }
}

void printRPSfromQEI(){
      pc.printf("QEI rps: %f \t", currentRPSValue);
      pc.printf("QEI count: %f \n\r", currentPosition);
      pc.printf("duty cycle: %f \n\r", oriDutyCycle);
}

// void PrintK(){
//       pc.printf ("Current Kc: %f \t", controller.getPParam());
// }


//**********************************************
//             Main Function                   *
//**********************************************
int main()
{
    pc.printf("Hello\n\r");
    orState = motorHome();
    pc.printf("Rotor origin: %x\n\r", orState);

    sampleRPS.attach(&getRPSfromQEI, RPS_SAMPLING_RATE);
    PrintRPS.attach(&printRPSfromQEI, 3);
    //PrintKpara.attach(&PrintK,2);

    //******* Setup threads for controller *******
    controlInit();
    PIDthread.start(controlR);

    while (1)
    {
        readPIrunMotor();
    }
}