#include "hFramework.h"
#include <Lego_Touch.h>
#include <stddef.h>
#include <stdio.h>
#include <hGPIO.h>

using namespace hSensors;
using namespace hFramework;

#define napedTyl hMot1
#define napedSrodek hMot2
#define podnosnikPrzod hMot3
#define podnosnikTyl hMot4
#define skrecanie hMot5
#define napedTyl2 hMot6

void hMain()
{
    podnosnikPrzod.setEncoderPolarity(Polarity::Reversed);  //changing encoder polarity (Polarity::Normal is default)
    podnosnikPrzod.setMotorPolarity(Polarity::Reversed);  //changing motor polarity

    podnosnikTyl.setEncoderPolarity(Polarity::Reversed);  //changing encoder polarity (Polarity::Normal is default)
    podnosnikTyl.setMotorPolarity(Polarity::Reversed);  //changing motor polarity

    hExt1.pin1.setIn_pu();      // guzik przod
    hExt1.pin2.setIn_pu();      // guzik srodek
    hExt1.pin3.setIn_pu();      // guzik tyl

    //SINLNIKI
    //1 - NAPED tyl
    //2 - NAPED srodek
    //3 - PONDOSNIK PRZOD
    //4 - PODNOSNIK TYL
    //5 - SKRECANIE
    //6 - naped tyl

    int liftPower = 600;
    int ridePower = 225;
	int pressPower = 55;

    //PIERWSSZY SCHOD
            //jazda do przodu poki guzik 1przod nie jest wcisniety
            while(hExt1.pin1.read() )
            {
                napedSrodek.setPower(-ridePower);
                napedTyl.setPower(-ridePower);
                napedTyl2.setPower(-ridePower);
                Serial.printf("szukam schodow \n");
            }
            napedTyl.setPower(0);
            napedTyl2.setPower(0);
            napedSrodek.setPower(0);

            //podnoszenie 1 podwozia po dojezdzie przodu do schodka
            while(!hExt1.pin1.read())
            {
                podnosnikPrzod.setPower(liftPower);
				napedSrodek.setPower(-pressPower);
                napedTyl.setPower(-pressPower);
                napedTyl2.setPower(-pressPower);
                Serial.printf("podnosze przod \n");
            }
            podnosnikPrzod.setPower(0);
            podnosnikPrzod.rotRel(1500, liftPower, true, INFINITE);

    while(true)
    {
        //jazda do przodu az do dotkniecia srodka ze schodkiem
        while(hExt1.pin2.read())
        {
            napedSrodek.setPower(-ridePower);
            napedTyl.setPower(-ridePower);
            napedTyl2.setPower(-ridePower);
            Serial.printf("jade do przodu  1\n");
        }
        napedTyl.setPower(0);
        napedTyl2.setPower(0);
        napedSrodek.setPower(0);

        //podnoszenie srodka po dojezdzie srodka
        while(!hExt1.pin2.read())
        {
            podnosnikPrzod.setPower(-liftPower);
            podnosnikTyl.setPower(liftPower);
            napedTyl.setPower(-pressPower);
            napedTyl2.setPower(-pressPower);
            Serial.printf("podnosze srodek \n");
        }
        podnosnikPrzod.setPower(0);
        podnosnikTyl.setPower(0);
        sys.delay_ms(2000);

 /*       for(int i=0; i<=5; i++)
        {
            podnosnikPrzod.rotRel(-210, 1000, true, INFINITE);
            podnosnikTyl.rotRel(210, 1000, true, INFINITE);
        } */

        //jazda do przodu az do dojazdu przodu po raz drugi
        while(hExt1.pin1.read())
        {
            napedSrodek.setPower(-ridePower);
            napedTyl.setPower(-ridePower);
            napedTyl2.setPower(-ridePower);
            Serial.printf("jade do przodu  2\n");
        }
        hMot1.setPower(0);
        hMot2.setPower(0);
        hMot6.setPower(0);

        //podnoszenie 1 podwozia po deojezdzie przodu do schodka
        while(!hExt1.pin1.read())
        {
            podnosnikPrzod.setPower(liftPower);
			napedSrodek.setPower(-pressPower);
            napedTyl.setPower(-pressPower);
            napedTyl2.setPower(-pressPower);
            Serial.printf("podnosze przod \n");
        }
        podnosnikPrzod.setPower(0);
        podnosnikPrzod.rotRel(1500, liftPower, true, INFINITE);

        //jazda do przodu az do dotkniecia tylu
        while(hExt1.pin3.read())
        {
            hMot1.setPower(-ridePower);
            hMot2.setPower(-ridePower);
            hMot6.setPower(-ridePower);
            Serial.printf("jade do przodu 3\n");
        }
        hMot1.setPower(0);
        hMot2.setPower(0);
        hMot6.setPower(0);

        //podnoszenie tylu po dojezdzie tylu do schodka
        while(!hExt1.pin3.read())
        {
            podnosnikTyl.setPower(-liftPower);
			napedSrodek.setPower(-pressPower);
            napedTyl.setPower(-pressPower);
            napedTyl2.setPower(-pressPower);
            Serial.printf("podnosze tyl \n");
        }
        podnosnikTyl.setPower(0);
        podnosnikTyl.rotRel(-1500, liftPower, true, INFINITE);
    }
}