#include "mbed.h"
#include "HCSR04.h"

#define DIST_OBSTACULO 10.0


//Serial pc(USBTX,USBRX);
InterruptIn encoderDireita(PTA13);
// Ticker periodic;
Ticker flip;

PwmOut IN1(PTA12); //amarelo PTA12
PwmOut IN2(PTA5);  //verde PTA5
PwmOut IN3(PTC8);  //azul PTC8
PwmOut IN4(PTD4);  //roxo PTA1

Serial pc(USBTX, USBRX); // tx, rx
HCSR04 sonar(PTA13, PTD5);

int xAlvo = 500;
int yAlvo = 500;
int xRestante = xAlvo;
int yRestante = yAlvo;
bool completo = false;
bool obstaculo = false;
int pwm = 5;
int pulsos = 0;

//////////////////////////////////////////////////////////////
/////////// encoder //////////////////////////////////////////
//////////////////////////////////////////////////////////////

void increment() {
    pulsos++;
}

//////////////////////////////////////////////////////////////
/////////// ultrasom /////////////////////////////////////////
//////////////////////////////////////////////////////////////

void chamaSonar(){
    //pc.printf("Leitura sonar: %.2f \n\r", sonar.getCm());
}

bool existeObstaculo() {
    pc.printf("tentativa 1...\r\n");
    bool tentativa1 = (sonar.getCm() < DIST_OBSTACULO);
    wait_ms(100);
    pc.printf("tentativa 2...\r\n");
    bool tentativa2 = (sonar.getCm() < DIST_OBSTACULO);
    wait_ms(100);
    pc.printf("tentativa 3...\r\n");
    bool tentativa3 = (sonar.getCm() < DIST_OBSTACULO);
    wait_ms(100);
    return tentativa1 || tentativa2 || tentativa3;
}

//////////////////////////////////////////////////////////////
/////////// motor ////////////////////////////////////////////
//////////////////////////////////////////////////////////////


void ptras(){
    IN1=0;
    IN2.pulsewidth_ms(pwm);
    IN3.pulsewidth_ms(pwm);
    IN4=0;
}

void pfrente(){
    IN1.pulsewidth_ms(pwm);
    IN2=0;
    IN3=0;
    IN4.pulsewidth_ms(pwm);
}

void stop(){
    //ptras();
    IN1=0;
    IN2=0;
    IN3=0;
    IN4=0;
}

void seguranca(){
    IN1.period_ms(10);
    IN2.period_ms(10);
    IN3.period_ms(10);
    IN4.period_ms(10);
    stop();
}
void g90esquerda(){
        IN1=0;
        IN2.pulsewidth_ms(pwm);
        IN3=0;
        IN4.pulsewidth_ms(pwm);
        wait(2);
    }

void g90direita(){
        IN1.pulsewidth_ms(pwm);
        IN2=0;
        IN3.pulsewidth_ms(pwm);
        IN4=0;
        wait(2);
    }

int sobramPulsosX() {
     return (pulsosEsq < xRestanteEsq) * 1 +  (pulsosDir < xRestanteDir) * 2;
}

int sobramPulsosY() {
     return (pulsosEsq < yRestanteEsq) * 1 +  (pulsosDir < yRestanteDir) * 2;
}

int motorFrente(int comando /* 1 apenas esq,  2 apenas dir, 3 ambos*/) {
    // Motor esquerdo
    if((comando == 1) || (comando == 3)) {
        IN1.pulsewidth_ms(pwm);
    } else {
        IN1 = 0;
    }
    IN2=0;
    // Motor direito
    IN3=0;
    if((comando == 2) || (comando == 3)) {
        IN4.pulsewidth_ms(pwm);
    } else {
        IN4 = 0;
    }
}


int main() {
    int pulsosRestantes;
    encoderDireita.rise(&increment);
    //flip.attach(&chamaSonar, 2.0);

    seguranca();
    while(!completo) {
        // Ir na direcao x ate acabar ou encontrar obstaculo
        pulsosRestantes = sobramPulsosX();
        while (pulsosRestantes && !obstaculo) {
            motorFrente(pulsosRestantes);
            pc.printf("obstaculo detectado: %s\r\n", obstaculo ? "sim" : "nao");
            obstaculo = existeObstaculo();
            wait_ms(500);
        }

//        while (pulsos < xRestante && !obstaculo) {
//            pc.printf("pfrente();\r\n");
//            obstaculo = existeObstaculo();
//            pc.printf("obstaculo detectado: %s\r\n", obstaculo ? "sim" : "nao");
//            wait_ms(500);
//        }
        // Atualizar condicoes
        xRestante = xRestante - pulsos;

        // Fazer curva
        wait(0.5);
        pc.printf("stop();\r\n");
        wait(3);
        pc.printf("g90esquerda();\r\n");
        pc.printf("stop();\r\n");
        wait(3);

        // Resetar variaveis
        pulsos = 0;
        obstaculo = false;

        // Ir na direcao y ate acabar ou encontrar obstaculo
        while (pulsos < yRestante && !obstaculo) {
            pc.printf("pfrente();\r\n");
            obstaculo = existeObstaculo();
            pc.printf("obstaculo detectado: %s\r\n", obstaculo ? "sim" : "nao");
            wait_ms(500);
        }
        // Atualizar condicoes
        yRestante = yRestante - pulsos;

        if (yRestante <= 0 && xRestante <= 0) {
            completo = true;
        } else {
            wait(0.5);
            pc.printf("stop();\r\n");
            wait(3);
            pc.printf("g90direita();\r\n");
            pc.printf("stop();\r\n");
            wait(3);
            pulsos = 0;
            obstaculo = false;
        }
    }
    pc.printf("stop();\r\n");
}