#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 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(40, 150, 0);

}
void turn_right()
{

 motor_turn(150, 40, 0);

}
void mild_turn_left()
{

motor_turn(70, 150, 0);

}
void mild_turn_right()
{

motor_turn(150, 60, 0);

}
void hard_turn_left()
{

motor_turn(0, 150, 0);

}
void hard_turn_right()
{

motor_turn(150, 0, 0);
}



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_left();
        }
        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_right();
        }
        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_left();
        }
         else if (dig.l3 == 1 && dig.l2 == 1 && dig.l1 == 1 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny vahva oikea
            hard_turn_left();
        }
         else if (dig.l3 == 1 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny vahva oikea
            hard_turn_left();
        }
        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 == 0 && dig.r2 == 1 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny vahva vasen
            hard_turn_right();
        }
        else if (dig.l3 == 0 && dig.l2 == 0 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && 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 == 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_right();
        }
        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_left();
        }
         else if (dig.l3 == 0 && dig.l2 == 1 && dig.l1 == 0 && dig.r1 == 0 && dig.r2 == 0 && dig.r3 == 0)
        {
            linjalla = false;
            // käänny loiva oikea
            mild_turn_left();
        }
        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(0,0);
                    linjalla = true;
                    linjalaskuri++;
                }
                //linjalaskuri++;
            }

            else
            {
                motor_forward(0,0);
            }
            /*
            if (!linjalla)
            {
                linjalaskuri++;
                linjalla = true;
            }
               */

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



    }
}
#endif