#include <hFramework.h>
#include <stdio.h>
#include <hMotor.h>
#include <stdio.h>
#include <hMotor.h>
#include <hExt.h>
#include <DistanceSensor.h>
#define clear() printf(" 33[H 33[J")
#include <MPU9250.h>


using namespace hFramework;
using namespace hModules;


DistanceSensor sens(hSens1);
DistanceSensor sens2(hSens3);

//odbieranie informacji o skrecaniu i flaga podnoszenia

char c;
bool flagaO=true;
bool flagaZ=false;
bool flagaG=false;
bool flagaD=false;
bool stop=false;
bool stopT=false;

//////////////////////////////////
void jazda()
{
    hExt1.serial.init(38400, NONE, ONE);
    bool flagaP;
    bool flagaL;

    while (true)
    {
        if (hExt1.serial.available() > 0)
        {
            c = (char) hExt1.serial.getch();

            switch (c)
            {
            case 'G':

                if(!stop)
               {
                    hMot2.setPower(700);
                    flagaG=true;
                }
            break;

            case 'D':
                 if(!stopT)
               {
                    hMot2.setPower(-700);
                    flagaD=true;
                    stop=false;
                }
            break;

            case 'B':
                hMot2.setPower(0);
                 flagaG=false;
                  flagaD=false;
            break;

            case 'L':
                hMot4.rotRel(-120,900);
                flagaP=false;
                flagaL=true;
                LED1.on();
            break;

            case 'P':
                hMot4.rotRel(120,900);
                flagaP=true;
                flagaL=false;
            break;
            case 'A':
                if(flagaP)
                {
                    hMot4.rotRel(-120,900);
                }
                else if(flagaL)
                {
                    hMot4.rotRel(120,900);

                }
                LED1.off();
            break;
                //podniesienie
            case 'O':
                if(flagaO)
                {
                    hMot6.rotRel(-350,1000);
                    hMot5.rotRel(-380,1000);
                    flagaO=false;
                    flagaZ=true;
                }
                LED3.on();
            break;
            //opuszczanie
            case 'Z':
                if(flagaZ)
                {
                    hMot6.rotRel(350,300);
                    hMot5.rotRel(380,300);
                    flagaO=true;
                    flagaZ=false;
                }
                LED3.on();
            break;
            case 'C':
                LED3.off();
            break;
            }
        }

    }
}
////////////////////////////
// odbieranie informacji od akcelerometra
void akcelerometr()
{
    int dist;
    int dist2;
    while(true)
    {
        dist2 = sens2.getDistance();
        dist = sens.getDistance();

        if((dist>0 && dist<40)&&flagaO&&flagaG)
        {
            if(dist2>41)
            {
               // stop=false;
                LED2.on();
                hMot6.rotRel(-350,1000);
                hMot5.rotRel(-380,1000);
                flagaO=false;
                flagaZ=true;
                stop=false;
             }
             else if(dist2<30)
             {
                hMot2.setPower(0);
                stop=true;
             }
        }
        else if(dist>40 && dist <100)
                LED2.off();
        else if (dist2<30&&flagaZ)
        {
                hMot2.setPower(0);
                stop=true;
             }

     //   sys.delay(50);
           // clear();
           // printf("pierwszy %d drugi %d",dist,dist2);

    }

}

void hMain(void)
{
    sys.setLogDev(&Serial);
    //tworzymy nowe taski
    sys.taskCreate(jazda, (uint8_t) 2);
    sys.taskCreate(akcelerometr, (uint8_t) 2);

    MPU9250 mpu(hSens2);
    mpu.init();
    mpu.enableInterrupt();
    bool flagACC = false;
    int16_t prev = 260;
    int16_t gy, gx, gz, ay, ax, az;
    uint32_t time=0;

    bool action = false;
    while(true)
    {

    ///////////////////////////////////////////////////////

////////////////////////////////////////////////
        mpu.waitForData();
         if (mpu.process())
        {

            MPU9250::Reading reading = mpu.getReading();
            gy=reading.gy;
            gz=reading.gz;
            gx=reading.gx;
            ay=reading.ay;
            az=reading.az;
            ax=reading.ax;

            if(flagaZ)
            {
            if(gx<-2400 || gx>2400)
                {
                        if(flagaG || flagaD)
                        {
                        action = true;
                        time = sys.getRefTime();
                        LED3.on();
                        }
                }
            else if(action && (sys.getRefTime() - time > 5000))
            {
                    hMot6.rotRel(350,300);
                    hMot5.rotRel(380,300);
                    flagaO=true;
                    flagaZ=false;
                    action = false;
            }
            else
                LED3.off();
            }

            else if(flagaO)
           {
                if(flagaD && gx>1000)
                {
                   hMot6.rotRel(-350,1000);
                    hMot5.rotRel(-380,1000);
                    flagaO=false;
                    flagaZ=true;
                }

           else if(flagaG && gx>760 )
                {
                        hMot6.rotRel(-350,1000);
                        hMot5.rotRel(-380,1000);
                        flagaO=false;
                        flagaZ=true;

                }
            }
            //sys.delay(50);
            //clear();
            //printf("gy %6d\r\ngx %6d\r\ngz %6d\r\nay %6d\r\naz %6d\r\nax %6d",gy, gx, gz,ay,az,ax);
        }
    }
}