#include "ASEIO.h"
#include<stdio.h>
#include<stdlib.h>

void FD(int s){
    SetMoto(0,s);
    SetMoto(1,-s);
}

void BK(int s){
    SetMoto(0,-s);
    SetMoto(1,s);
}
void RT(int s){
    SetMoto(0,s);
    SetMoto(1,s);
}
void LT(int s){
    SetMoto(0,-s);
    SetMoto(1,-s);
}

void lineway_time(float i){
    resettime();
    float tim_1 = 0.0f;
    while(tim_1 < i)
    {
        if (AI(0)>ReadEEPROM(0) || AI(4)>ReadEEPROM(0))
            FD(250);
        else if(AI(1) > ReadEEPROM(0)) {
            LT(250);
        } else if (AI(3) > ReadEEPROM(0)) {
            RT(250);
        } else if(AI(2) > ReadEEPROM(0)) {
            FD(250);
        }
        tim_1=seconds();
    }
}


void lineway_cross( int i) {
    while(AI(i)< ReadEEPROM(0))
    {
        if(AI(1) > ReadEEPROM(0)){SetMoto(0,-250);SetMoto(1,-250);}
        else if(AI(3) > ReadEEPROM(0)){SetMoto(0,250);SetMoto(1,250);}
        else if(AI(2) > ReadEEPROM(0)){SetMoto(0,250);SetMoto(1,-250);}
        else ;
    }
    while(AI(0) > ReadEEPROM(0)){}
    SetMoto(0,0);SetMoto(1,0);
}

void turn_around( int i)
{
    if(i==0)
    {
        FD(250);
        wait(0.2);
        LT(250);
        while (AI(2)>ReadEEPROM(0));
        while (AI(2)<ReadEEPROM(0));
        FD(0);
    }
    else if(i==4)
    {
        FD(250);
        wait(0.2);
        RT(250);
        while (AI(2)>ReadEEPROM(0));
        while (AI(2)<ReadEEPROM(0));
        FD(0);
    }
}

void lineway(){
    int flag = 1;
    while(flag == 1)
    {
        if((AI(1) < ReadEEPROM(0) && AI(2) < ReadEEPROM(0) && AI(3) < ReadEEPROM(0)))
        {
            FD(0);
            break;
        }
        if (AI(0)>ReadEEPROM(0) || AI(4)>ReadEEPROM(0)){
            FD(250);
        }
        else if(AI(1) > ReadEEPROM(0)) {
            LT(250);
        } else if (AI(3) > ReadEEPROM(0)) {
            RT(250);
        } else if(AI(2) > ReadEEPROM(0)) {
            FD(250);
        }
    }
}

void energy9(){
    lineway_time(2.5);
    RT(250);
    wait(0.5);
    while (AI(2) > ReadEEPROM(0));
    while (AI(2) < ReadEEPROM(0));
    lineway_time(1);
    BK(250);
    wait(4);
    BK(0);
}

void energy8(){
    SetMoto(1,-150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    int flag = 1;
    lineway_time(0.6);
    BK(250);
    wait(3.5);
    BK(0);
}

void energy7(){
    SetMoto(0,150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    int flag = 1;
    lineway_time(0.55);
    BK(250);
    wait(3.5);
    BK(0);
}

void energy6(){
    SetMoto(1,-150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    int flag = 1;
    lineway_time(1);
    lineway();
    BK(250);
    wait(4);
    BK(0);
}

void energy4(){
    SetMoto(1,-150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    lineway_time(0.15);
    LT(250);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    lineway_time(0.8);
    BK(250);
    wait(3);
    BK(0);
}

void energy3(){
    SetMoto(0,150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    int flag = 1;
    lineway_time(1);
    BK(250);
    wait(2);
    wait(4);
    BK(0);
}

void energy2(){
    SetMoto(0,150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    int flag = 1;
    lineway_time(0.3);
    lineway();
    BK(250);
    wait(4);
    BK(0);
}

void energy1(){
    SetMoto(1,-150);
    wait(0.3);
    while (AI(2) < ReadEEPROM(0));
    int flag = 1;
    lineway_time(0.8);
    BK(250);
    wait(5);
    BK(0);
}

void WER(){
    lineway_cross(4);
    energy4();
}

void main()
{
    FD(250);
    wait(0.3);
    WER();
}