#include <kipr/wombat.h>

// ============================================================
// ET SENSOR - DISTANCE
// ============================================================
// Drive forward until ET sensor reads an obstacle (analog > 2900)
void et_sensor_example() {
    while (analog(1) <= 2900) {
        motor(0, 50);
        motor(3, 50);
    }
    ao();
    msleep(100);
}

// ============================================================
// IR SENSOR / TOPHAT (Large or Small)
// ============================================================

// Find black: move forward until black detected, then back up off it
void find_black() {
    // Move forward until black is detected
    while (analog(1) <= 1580) {
        printf("start loop move forward\n");
        motor(0, 80);
        motor(3, 80);
    }
    // Move backward until black is no longer detected
    while (analog(1) >= 1580) {
        printf("start loop move backward\n");
        motor(0, -80);
        motor(3, -80);
    }
    printf("start loop stops\n");
    ao();
    msleep(100);
}

// Follow the line (single sensor, basic)
void follow_line_basic() {
    cmpc(0);
    while (gmpc(0) < 40000) {
        if (analog(1) < 3500) {
            motor(0, 80);
            motor(3, 5);
        } else {
            motor(0, 5);
            motor(3, 80);
        }
    }
}

// Follow the line (single sensor, race speed with correction)
void follow_line_race() {
    printf("follow the line 23.95\n");
    int right = 0;
    int left = 0;
    cmpc(0);
    while (gmpc(0) < 20000) {
        if (analog(0) > 3750) {
            motor(0, 25 - right);
            motor(3, 100);
            msleep(1);
            right = right + 1;
            left = left - 1;
            msleep(1);
        }
        if (analog(0) < 3750) {
            motor(3, 25 - left);
            motor(0, 100);
            msleep(1);
            left = left + 1;
            right = right - 1;
            msleep(1);
        }
        printf("left %d and right %d\n", left, right);
    }
}

// Follow the line (two sensors, with lever sensor stop)
void follow_line_two_sensors() {
    int difference = 2000;
    while (digital(0) == 0) { // while lever sensor is not pressed
        if (analog(1) <= difference && analog(0) <= difference) {
            // Both sensors on white: go forward
            motor(0, 50);
            motor(3, 50);
            msleep(25);
            printf("1\n");
        } else if (analog(1) <= difference) {
            // Sensor 1 on white: turn right
            motor(0, -10);
            motor(3, 50);
            msleep(25);
            printf("2\n");
        } else if (analog(0) <= difference) {
            // Sensor 0 on white: turn left
            motor(0, 50);
            motor(3, -10);
            msleep(25);
            printf("3\n");
        } else {
            // Both sensors on black: go forward
            motor(0, 100);
            motor(3, 100);
            msleep(25);
            printf("4\n");
        }
    }
}