void motor_forward_fixed(uint8 speed, uint32 delay)
{
    MotorDirLeft_Write(0);
    MotorDirRight_Write(0);
    float ratio = 1.05; // pitää säätää, 1.3 vähän liikaa

    uint8 l_speed = speed;
    uint8 r_speed = (float)speed / ratio;

    printf("l_speed: %d, r_speed: %d\n", l_speed, r_speed); // tulostaa nopeudet

    motor_turn(l_speed, r_speed, delay);
}
void motor_tank_turn_90dg(int left_dir, int right_dir)
    {
        MotorDirLeft_Write(left_dir);
        MotorDirRight_Write(right_dir);
        PWM_WriteCompare1(75);
        PWM_WriteCompare2(75);
        vTaskDelay(1500);
    }

void turn_left()
{

  motor_turn(20, 100, 50);

}
void turn_right()
{

 motor_turn(100, 20, 50);

}
void mild_turn_left()
{

    motor_turn(40, 100, 50);

}
void mild_turn_right()
{

    motor_turn(100, 40, 50);

}
void hard_turn_left()
{

motor_turn(0, 100, 50);

}
void hard_turn_right()
{

motor_turn(100, 0, 50);
}



void zmain(void)
{
    IR_Start(); //käynnistetään IR sensori
    IR_flush();

    reflectance_start(); //Käynnistetään reflectance sensorit
    //struct sensors_ ref;
    struct sensors_ dig;
    reflectance_set_threshold(9000, 9000, 11000, 11000, 9000, 9000);

    motor_start(); //käynnistetään moottori
    motor_forward(0, 0); //pysäytetään moottorit

    int linjalaskuri = 0;
    bool linjalla = false;

    bool led = false; //ledi päälle kun robotti saa IR arvon

    while (SW1_Read()==1)
    {
        vTaskDelay(200);
    }

    vTaskDelay(1000);

    for(;;)
    {
        reflectance_digital(&dig); // dig.l3, dig.l2, dig.l1, dig.r1, dig.r2, dig.r3
        motor_forward_fixed(70,0);

        // VIIVAN SEURANTA

        if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // mene suoraan
            motor_forward_fixed(70, 0);
        }
        else if (dig.l3 == 0 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny oikea
            turn_right();
        }
        else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny vasen
            turn_left();
        }
        else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny vahva oikea
            hard_turn_right();
        }
        else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 1 && dig.r3 == 1)
        {
            linjalla = false;
            // käänny vahva vasen
            hard_turn_right();
        }
        else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 1 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny loiva vasen
            mild_turn_left();
        }
        else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny loiva oikea
            mild_turn_right();
        }
        else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 1 && dig.r2 == 1 && dig.r3 == 1)
        {
            // kaikki on ykkösiä
            if (linjalaskuri == 0)
            {
                motor_forward(0, 0);
                IR_wait();
                led = !led;
                linjalla = true;
                linjalaskuri++;
            }
            else if (linjalaskuri == 1)
            {
                if (!linjalla)
                {
                    motor_forward_fixed(70, 200);
                    motor_tank_turn_90dg(1, 0);
                    linjalla = true;
                    linjalaskuri++;
                }
                //linjalaskuri++;
            }
            else if (linjalaskuri <= 3)
            {
                if (!linjalla)
                {
                    motor_forward_fixed(70, 200);
                    motor_tank_turn_90dg(0, 1);
                    linjalla = true;
                    linjalaskuri++;
                }
                //linjalaskuri++;
            }
            else
            {
                motor_forward(0,0);
            }
            /*
            if (!linjalla)
            {
                linjalaskuri++;
                linjalla = true;
            }
               */

        }
        else
        {
            motor_forward_fixed(70,0);
        }
            // lisää iffejä...


        vTaskDelay(50);
    }
}