#include <project.h>
#include <stdio.h>
#include "FreeRTOS.h"
#include "task.h"
#include "Motor.h"
#include "Ultra.h"
#include "Nunchuk.h"
#include "Reflectance.h"
#include "Gyro.h"
#include "Accel_magnet.h"
#include "LSM303D.h"
#include "IR.h"
#include "Beep.h"
#include "mqtt_sender.h"
#include <time.h>
#include <sys/time.h>
#include "serial1.h"
#include <unistd.h>

#if 1

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 turn_left()
{

  motor_turn(20, 70, 200);

}
void turn_right()
{

 motor_turn(70, 20, 200);

}

void mild_turn_left()
{

  motor_turn(30, 70, 100);

}

void mild_turn_right()
{

motor_turn(70, 30, 100);

}

void hard_turn_left()
{

motor_turn(0, 70, 500);

}


void hard_turn_right()
{

motor_turn(70, 0, 500);
}



 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 led = false; //ledi päälle kun robotti saa IR arvon

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

   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)
        {
            // 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)
        {
            // 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)
        {
            // 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)
        {
            // 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)
        {
            // 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)
        {
            // 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)
        {
            // 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;
                linjalaskuri++;
            }
            else if(linjalaskuri<=4)
            {
                motor_forward_fixed(100,0);
                linjalaskuri++;
            }
            else
            {
                motor_forward(0,0);
            }
            // odota IR tai linjalaskuri++;
        }
        else
        {
            motor_forward_fixed(70,0);
        }
            // lisää iffejä...


        vTaskDelay(50);
    }
}
#endif