#include <hFramework.h>
#include <stdio.h>

#include "Hitechnic_ColorV2.h"
#include "Lego_Touch.h"

using namespace hSensors;
using namespace hFramework;


void encoder()
{
    while (true)
    {
        // Print the current rotation of hMot1 in encoder ticks
        //printf("pos: %d\r\n", hMot1.getEncoderCnt());
        LED1.toggle();
        sys.delay(100);
    }
}

void hMain(void)
{

sys.taskCreate(encoder);

    Hitechnic_ColorV2 sensor(hSens1);
    sys.setLogDev(&Serial);
    Lego_Touch sensor1(hSens2);

    bool pressed = sensor1.isPressed();

    while(!pressed){
    pressed = sensor1.isPressed();
    hMot1.setPower(400);
    //printf("state %d\r\n", pressed);
    sys.delay_ms(10);

    }

    hMot1.setPower(0);
    hMot1.resetEncoderCnt();

    for (;;)
    {


        int white;
        float hue, saturation, value;
        uint8_t r, g, b;
        sensor.readHSV(hue, saturation, value);
        sensor.readWhite(white);
        sensor.readRGB(r, g, b);
        LED1.toggle();
        printf("hue %1f saturation %1f value %1f\r\n", hue, saturation, value);
        printf("r %4d g %4d b %4d\r\n", r, g, b);
        printf("n %d ", white);
        sys.delay_ms(10);

        //biały na dół
        if(230<=white){
        hMot1.rotAbs(-16);
        sys.delay(500);
        hMot2.rotRel(-360,700);
        sys.delay_ms(1000);

        }

       //czarny na górę
        else if(white<100 && value < 20){
        hMot1.rotAbs(-410);
        sys.delay(500);
        hMot2.rotRel(-360,700);
        sys.delay_ms(1000);
        }

        //zielony na środek
        else if(100<white && white <165){
        hMot1.rotAbs(-170);
        sys.delay(500);
        hMot2.rotRel(-360,700);
        sys.delay_ms(1000);
        }

        //powietrze
        else if(hue<50)
        {
        hMot2.setPower(0);
        }

        else {
        hMot2.rotRel(100,400);
        hMot2.rotRel(-100,400);
        sys.delay_ms(500);
        }

    }

}