#include <stdio.h>
#include <stdlib.h>
#include <time.h>

#include "pico/multicore.h"
#include "pico/stdlib.h"
#include "pico/time.h"

#define PWM_RANGE 255

// Pico LED
const uint ONBOARD_LED = 25;

// Motor pins
const uint IN1 = 6;
const uint IN2 = 7;
const uint EN_A = 8;
const uint IN3 = 4;
const uint IN4 = 3;
const uint EN_B = 2;

// Infrared sensor pins
const uint IR_RIGHT = 27;
const uint IR_LEFT = 26;

// Untrasonic sensor pins
const uint ECHO = 19;
const uint TRIG = 18;

// PWM generated output
int PWM_OUT = 0;
int PWM_OUT2 = 0;
int PWM_OUT3 = 0;

void left() {
    gpio_put(IN1, PWM_OUT2);
    gpio_put(IN2, 0);
    gpio_put(IN3, 0);
    gpio_put(IN4, PWM_OUT2);
}

void right() {
    gpio_put(IN1, 0);
    gpio_put(IN2, PWM_OUT);
    gpio_put(IN3, PWM_OUT);
    gpio_put(IN4, 0);
}

void forward() {
    gpio_put(IN1, PWM_OUT3);
    gpio_put(IN2, 0);
    gpio_put(IN3, PWM_OUT3);
    gpio_put(IN4, 0);
}

void back() {
    gpio_put(IN1, 0);
    gpio_put(IN2, 1);
    gpio_put(IN3, 0);
    gpio_put(IN4, 1);
}

void stop() {
    gpio_put(IN1, 0);
    gpio_put(IN2, 0);
    gpio_put(IN3, 0);
    gpio_put(IN4, 0);
}

float get_distance() {
    uint32_t echo_timeout = 3000;
    gpio_put(TRIG, 0);
    sleep_ms(50);
    gpio_put(TRIG, 1);
    sleep_ms(50);
    gpio_put(TRIG, 0);

    uint64_t start = time_us_64();
    uint64_t end = start;

    while (gpio_get(ECHO) == 0) {
        if (time_us_64() - start > echo_timeout) {
            return -1;
        }
        start = time_us_64();
    }

    while (gpio_get(ECHO) == 1) {
        end = time_us_64();
        if (time_us_64() - start > echo_timeout) {
            return -1;
        }
    }

    float cms = (float)(end - start) / 2 / 29.1;
    if (cms > 0) {
        return cms;
    }
    return -1;
}

int main() {
    stdio_init_all();
    
    gpio_init(ONBOARD_LED);
    gpio_init(IN1);
    gpio_init(IN2);
    gpio_init(EN_A);
    gpio_init(IN3);
    gpio_init(IN4);
    gpio_init(EN_B);
    gpio_init(IR_RIGHT);
    gpio_init(IR_LEFT);
    gpio_init(ECHO);
    gpio_init(TRIG);

    gpio_set_dir(ONBOARD_LED, GPIO_OUT);
    gpio_set_dir(IN1, GPIO_OUT);
    gpio_set_dir(IN2, GPIO_OUT);
    gpio_set_dir(EN_A, GPIO_OUT);
    gpio_set_dir(IN3, GPIO_OUT);
    gpio_set_dir(IN4, GPIO_OUT);
    gpio_set_dir(EN_B, GPIO_OUT);
    gpio_set_dir(IR_RIGHT, GPIO_IN);
    gpio_set_dir(IR_LEFT, GPIO_IN);
    gpio_set_dir(ECHO, GPIO_IN);
    gpio_set_dir(TRIG, GPIO_OUT);
    
    gpio_put(EN_A, 1);
    gpio_put(EN_B, 1);
    
     gpio_set_function(PWM_OUT, GPIO_FUNC_PWM);
    gpio_set_pwm_range(PWM_OUT, PWM_RANGE);
    gpio_set_pwm_frequency(PWM_OUT, 1000);

     gpio_set_function(PWM_OUT2, GPIO_FUNC_PWM);
    gpio_set_pwm_range(PWM_OUT2, PWM_RANGE);
    gpio_set_pwm_frequency(PWM_OUT2, 1000);

     gpio_set_function(PWM_OUT3, GPIO_FUNC_PWM);
    gpio_set_pwm_range(PWM_OUT3, PWM_RANGE);
    gpio_set_pwm_frequency(PWM_OUT3, 1000);

     gpio_set_function(PWM_OUT4, GPIO_FUNC_PWM);
    gpio_set_pwm_range(PWM_OUT4, PWM_RANGE);
    gpio_set_pwm_frequency(PWM_OUT4, 1000);

    while (true) {
        gpio_put(ONBOARD_LED, 1);

        bool ir_right_state = gpio_get(IR_RIGHT);
        bool ir_left_state = gpio_get(IR_LEFT);

        float distance = get_distance();

        if (distance <= 15 && distance != -1) {
            stop();
            sleep_ms(100);
        } else {
            if (ir_right_state == 0 && ir_left_state == 1) {
                while (gpio_get(IR_RIGHT) != 0 ||  gpio_get(IR_LEFT) != 1) {
                    for (int i = 0; i <= PWM_RANGE; i++) {
                        gpio_set_pwm_level(PWM_OUT, i);
                        sleep_ms(10);
                    }
                    for (int i = PWM_RANGE; i >= 0; i--) {
                        gpio_set_pwm_level(PWM_OUT, i);
                        sleep_ms(10);
                        }
                        right();
                    }
            }
            if (ir_right_state == 1 && ir_left_state == 0) {
                while (gpio_get(IR_RIGHT) != 1 || gpio_get(IR_LEFT) != 0) {
                    for (int i = 0; i <= PWM_RANGE; i++) {
                        gpio_set_pwm_level(PWM_OUT2, i);
                        sleep_ms(10);
                    }
                    for (int i = PWM_RANGE; i >= 0; i--) {
                        gpio_set_pwm_level(PWM_OUT2, i);
                        sleep_ms(10);
                        }
                        left();
                    }
            }
            if (ir_right_state == 1 && ir_left_state == 1) {
                stop();
            }
            if (ir_right_state == 0 && ir_left_state == 0) {
                while (gpio_get(IR_RIGHT) != 0 || gpio_get(IR_LEFT) != 0) {
                    for (int i = 0; i <= PWM_RANGE; i++) {
                        gpio_set_pwm_level(PWM_OUT3, i);
                        sleep_ms(10);
                    }
                    for (int i = PWM_RANGE; i >= 0; i--) {
                        gpio_set_pwm_level(PWM_OUT3, i);
                        sleep_ms(10);
                        }
                       forward();
                    }
                
            }
        }
        
        sleep_ms(5);
    }

    return 0;
}