#include <kipr/wombat.h>

// ============================================================
// BACKWARD
// ============================================================
void backward() {
    motor(0, -100);
    motor(1, -100);
    motor(2, -100);
    motor(3, -100);
    msleep(3000);
}

// ============================================================
// FORWARD
// ============================================================
void forward() {
    motor(1, 100);
    motor(2, 100);
    motor(3, 100);
    msleep(3000);
}

// ============================================================
// LEFT DIAGONAL
// ============================================================
// ⚠️ NOTE: This function is named turn_left_90 but implements
// a LEFT DIAGONAL movement, not a 90-degree turn. Rename before
// using in competition code.
void turn_left_90_LEFT_DIAGONAL() {
    cmpc(1);
    while (gmpc(1) < 1350) {
        motor(0, 0);
        motor(1, 100);
        motor(2, 0);
        motor(3, 100);
    }
    ao();
}

// ============================================================
// LEFT SIDEWAYS
// ============================================================
// ⚠️ NOTE: This function is named turn_left_90 but implements
// a LEFT SIDEWAYS (strafe) movement. Rename before using in
// competition code.
void turn_left_90_LEFT_SIDEWAYS() {
    cmpc(0);
    while (gmpc(0) < 1350) {
        motor(0, 100);
        motor(1, -100);
        motor(2, 100);
        motor(3, -100);
    }
    ao();
}

// ============================================================
// RIGHT DIAGONAL
// ============================================================
// ⚠️ NOTE: This function is named turn_left_90 but implements
// a RIGHT DIAGONAL movement. Rename before using in competition
// code.
void turn_left_90_RIGHT_DIAGONAL() {
    cmpc(0);
    while (gmpc(0) < 1350) {
        motor(0, 100);
        motor(1, 0);
        motor(2, 100);
        motor(3, 0);
    }
    ao();
}

// ============================================================
// RIGHT SIDEWAYS
// ============================================================
// ⚠️ NOTE: This function is named turn_left_90 but implements
// a RIGHT SIDEWAYS (strafe) movement. Rename before using in
// competition code.
void turn_left_90_RIGHT_SIDEWAYS() {
    cmpc(0);
    while (gmpc(0) < 1350) {
        motor(0, -100);
        motor(1, 100);
        motor(2, -100);
        motor(3, 100);
    }
    ao();
}

// ============================================================
// MOTOR POSITION COUNTER (FORWARD WITH MPC)
// ============================================================
// ⚠️ NOTE: This snippet has structural issues — the forward()
// function is defined inside turn_left_90() and the code is
// missing closing braces. Needs a full rewrite before use.
// Preserved here as-is for reference only.
/*
void turn_left_90_MPC_BROKEN() {
    cmpc(0);
    while (gmpc(0) < 1350) {
        motor(0, 100);
        motor(1, -100);
        motor(2, 100);
        motor(3, -100);
        ao();
    }
    void forward() {          // <-- function defined inside function: NOT valid C
        cmpc(0);
        while (gmpc(0) < 2000) {
            motor(0, 100);
            motor(1, 100);
            motor(2, 100);
            motor(3, 100);
            msleep(3000);
            ao();
            // missing closing braces
*/