#include "mbed.h"
#include "C12832.h"
#include "QEI.h"
DigitalOut enable(PB_2);
class motormovement
{
private:
    PwmOut pwm1, pwm2;
    DigitalOut bipolar1, bipolar2;
public:
    motormovement(PinName a, PinName b, PinName c, PinName d) :pwm1(a), pwm2(b), bipolar1(c), bipolar2(d)
    {
        bipolar1.write(1);
        bipolar2.write(1);
    };
    void moveleft(float lpwm)
    {


        pwm1.period(0.00005);
        pwm1.write(lpwm);

    }
    void moveright(float rpwm)
    {

        pwm2.period(0.00005);
        pwm2.write(rpwm);
    }
    void stop()
    {
        enable.write(0);
    }

};
class velocity
{
private:
    Ticker ticker;//ticker for encoder;
    QEI*encoderl;
    QEI*encoderr;
    float T, cetl, etrl, petl, aml, awl;
    float cetr, ertr, petr, amr, awr;
    float vl, vr, rav;
public:
    velocity(QEI*e1, QEI*e2, float t) :encoderl(e1), encoderr(e2), T(t)
    {
        petl = 0;
        petr = 0;
        //enable.write(1);
        ticker.attach(callback(this, &velocity::VELO), T);
    };

    void VELO()//caculate encoer ticker rate
    {

        cetl = encoderl->getPulses(); //current encoder ticker(left)
        etrl = (cetl - petl) / T;//encoder ticker rate(left)
        petl = cetl;//previous encoder ticker (left)
        aml = 2 * 3.14159*etrl / 256;    //get the angular speed for motor(left)
        awl = aml * 10 * 16 / 50 / 60;  //convert the angular speed for motor to angular speed for wheel(left)
        vl = awl * 0.045f; //coverted to line velocity(left)
        cetr = encoderr->getPulses();
        ertr = (cetr - petr) / T;
        petr = cetr;
        amr = 2 * 3.14159*ertr / 256;
        awr = amr * 10 * 16 / 50 / 60;
        vr = awr * 0.045f;
        rav = (vr - vl) / 0.2f;//calculate robert angular velocity
        //lcd.cls(); //clear the screen

    }

    float get_lpulse()
    {
        return cetl;
    }
    float get_rpulse()
    {
        return cetr;
    }
    float get_vl()
    {
        return vl;    //get the line velocity for wheel
    }
    float get_vr()
    {
        return vr;
    }
    float get_rav()
    {
        return rav;
    }
};
class display
{
private:
    C12832*lcd;
    float m_vl, m_vr;
public:
    display(C12832*c1, float vl, float vr) :lcd(c1), m_vl(vl), m_vr(vr) {}
    void disvel()
    {
        lcd->locate(10, 1); //locate at (0,3)
        lcd->printf("left velocity is:%.3f ", m_vl);
        lcd->locate(10, 11); //locate at (12,3)
        lcd->printf("right velocity is:%.3f ", m_vr);
    }
};
class Pspeed
{
private:
    float readvalue;
    float Kp;
    float setpoint, tp;
    float error;
    float perror;
    float kd;
    float Turn, wishV;
    float pwmr, pwml, difl, difr, vellp, velrp, vell, velr,  maxpwm, minpwm;
public:
    Pspeed()
    {
        error = 0;
        perror=0;
        Kp = 2.5;
        kd= 10;
        setpoint = 0.035;//the normal velocity
        tp = 0.01;//the pwm for the normal velocity
        difl=0;
        difr=0;
        pwml= 0.65;
        pwmr= 0.65;
        velrp=0;
        vellp=0;
        wishV= 0.7;
    }
    /* void calpid(float readvalue)
     {

         error = setpoint - readvalue;
         Turn = Kp * error+ (error-perror)*kd;
         perror= error;
         pwm = Tp + Turn;

     }
     */
    void drive(float vell, float velr)
    {

        difl= vell - vellp;
        difr= velr - velrp;
        vellp= vell;
        velrp=velr;

        if(( (vell||velr)< wishV)) {

            pwml += tp;
            pwmr += tp;

        }

        else if (( (vell||velr)> wishV)) {
            pwml -= tp;
            pwmr -= tp;
        } else {

            if (difl> difr) {
                pwml -= tp;
                pwmr += tp;
            } else if (difl< difr) {
                pwml += tp;
                pwmr -= tp;
            }
        }

    }
    float getpwml()
    {

        return pwml;
    }

    float getpwmr()
    {

        return pwmr;
    }


};
int main()
{
    float  vl, vr, rav, pulse,rpwm,lpwm;
    //vl:left velocity, vr:right velocity ,st:straghtline time ,tt:time needed to turn pi/2 ,tat:time needed to turn pi
    motormovement*wheel = new motormovement(PB_14, PB_15, PC_14, PC_15);//connect pwm1 to PB_14,pwm2 to PB_15 ,bipolar1 to PC_14,bipolar2 to PC_15 )
    QEI*e1 = new QEI(PC_10, PC_12, NC, 256);//left encoder
    QEI*e2 = new QEI(PC_11, PD_2, NC, 256);//right encoder
    C12832* c1 = new C12832(D11, D13, D12, D7, D10);

    velocity encoder(e1, e2, 0.01);
    Pspeed esl, esr;

    enable.write(1);
    while (1) {
        vl = encoder.get_vl();
        vr = encoder.get_vr();
        display mylcd(c1,vl,vr);
        mylcd.disvel();

        esl.drive(vl,vr);

        lpwm=esl.getpwml();
        rpwm = esr.getpwmr();
        wheel->moveleft(lpwm);
        wheel->moveright(rpwm);


        /*  esr.calpid(vr);


          if (((vl&&vr)<0.0035)) {
              lpwm=esl.getpwm()+0.01;
              rpwm = esr.getpwm()+0.01;
              wheel->moveleft(lpwm);
              wheel->moveright(rpwm);
          } else if (((vl&&vr)>0.0035)) {
              lpwm=esl.getpwm()-0.01;
              rpwm = esr.getpwm()-0.01;
              wheel->moveleft(lpwm);
              wheel->moveright(rpwm);
          } else if(((vl&&vr)==0.0035)){
          lpwm=esl.getpwm();
          rpwm = esr.getpwm();
          wheel->moveleft(lpwm);
          wheel->moveright(rpwm);
          }*/
    }



}