// ============================================================
// SELF-TEST: Mecanum Robot
// Run this before every practice and competition to verify
// all hardware is working correctly.
//
// HOW TO USE:
//   1. Update the port #defines below to match your robot
//   2. Set TEST_MOTORS / TEST_SERVOS / TEST_SENSORS to 0
//      if you want to skip that section
//   3. Set any unused port to -1 to skip it
//   4. Run the program — press the A button to advance
//      between tests so you can watch each component
//
// OUTPUT: Open the Console window in the Wombat software
//         to see all printed messages.
//
// MECANUM MOTOR LAYOUT (top view):
//
//              FRONT
//
//      LF (0)       RF (1)
//
//      LB (2)       RB (3)
//
//              BACK
//
// ============================================================

#include <kipr/wombat.h>

// ============================================================
// --- WHAT TO TEST (set to 0 to skip a whole section) ---
// ============================================================
#define TEST_MOTORS   1
#define TEST_SERVOS   1
#define TEST_SENSORS  1

// ============================================================
// --- PORT ASSIGNMENTS ---
// Use -1 if you DON'T have something plugged in.
// ============================================================

// Drive motors — update these if yours are wired differently
#define MOTOR_LF    0    // Left Front
#define MOTOR_RF    1    // Right Front
#define MOTOR_LB    2    // Left Back
#define MOTOR_RB    3    // Right Back

// Extra motors (set to -1 to skip)
#define EXTRA_MOTOR_A  -1
#define EXTRA_MOTOR_B  -1

// Servos (set to -1 to skip)
#define SERVO_0     0
#define SERVO_1     1
#define SERVO_2     2
#define SERVO_3     3

// Analog sensors (set to -1 to skip)
#define ANALOG_0    0
#define ANALOG_1    1
#define ANALOG_2    2
#define ANALOG_3   -1

// Digital sensors (set to -1 to skip)
#define DIGITAL_0   0
#define DIGITAL_1   1
#define DIGITAL_2  -1
#define DIGITAL_3  -1

// Speed for motor tests — keep low so robot doesn't travel far
#define TEST_SPEED  60
#define TEST_TIME   1200   // ms per direction

// ============================================================
// --- HELPERS ---
// ============================================================

void wait_for_a() {
    printf("  [ Press A button to continue ]\n");
    while (!a_button()) { msleep(50); }
    msleep(400);
}

void section_header(const char* title) {
    printf("\n=========================\n");
    printf("  %s\n", title);
    printf("=========================\n");
}

void set_drive(int lf, int rf, int lb, int rb) {
    motor(MOTOR_LF, lf);
    motor(MOTOR_RF, rf);
    motor(MOTOR_LB, lb);
    motor(MOTOR_RB, rb);
}

// ============================================================
// --- TEST FUNCTIONS ---
// ============================================================

void test_single_motor(int port, const char* name) {
    if (port < 0) return;

    printf("\nMotor: %s (port %d)\n", name, port);
    printf("  EXPECTED: Only this wheel spins.\n");
    wait_for_a();

    printf("  Forward (%d)...\n", TEST_SPEED);
    printf("  EXPECTED: Wheel spins forward.\n");
    motor(port, TEST_SPEED);
    msleep(TEST_TIME);
    ao();
    msleep(300);

    printf("  Backward (-%d)...\n", TEST_SPEED);
    printf("  EXPECTED: Wheel spins backward.\n");
    motor(port, -TEST_SPEED);
    msleep(TEST_TIME);
    ao();
    msleep(300);

    printf("  Done with %s.\n", name);
}

void test_drive_directions() {
    int s = TEST_SPEED;

    printf("\nForward\n");
    printf("  EXPECTED: Robot moves straight forward.\n");
    wait_for_a();
    set_drive(s, s, s, s);
    msleep(TEST_TIME);
    ao(); msleep(500);

    printf("\nBackward\n");
    printf("  EXPECTED: Robot moves straight backward.\n");
    wait_for_a();
    set_drive(-s, -s, -s, -s);
    msleep(TEST_TIME);
    ao(); msleep(500);

    printf("\nStrafe Right\n");
    printf("  EXPECTED: Robot slides right with NO rotation.\n");
    printf("  If it spins, a motor direction is wrong - note which ones.\n");
    wait_for_a();
    set_drive(s, -s, -s, s);
    msleep(TEST_TIME);
    ao(); msleep(500);

    printf("\nStrafe Left\n");
    printf("  EXPECTED: Robot slides left with NO rotation.\n");
    wait_for_a();
    set_drive(-s, s, s, -s);
    msleep(TEST_TIME);
    ao(); msleep(500);

    printf("\nRotate Right\n");
    printf("  EXPECTED: Robot spins clockwise in place.\n");
    wait_for_a();
    set_drive(s, -s, s, -s);
    msleep(TEST_TIME);
    ao(); msleep(500);

    printf("\nRotate Left\n");
    printf("  EXPECTED: Robot spins counter-clockwise in place.\n");
    wait_for_a();
    set_drive(-s, s, -s, s);
    msleep(TEST_TIME);
    ao(); msleep(500);
}

void test_servo(int port, const char* name) {
    if (port < 0) return;

    printf("\nServo: %s (port %d)\n", name, port);
    printf("  EXPECTED: Servo sweeps from one end to the other and back to center.\n");
    wait_for_a();

    enable_servo(port);

    printf("  Moving to 0...\n");
    set_servo_position(port, 0);
    msleep(1500);

    printf("  Moving to 2047...\n");
    set_servo_position(port, 2047);
    msleep(1500);

    printf("  Moving to center (1024)...\n");
    set_servo_position(port, 1024);
    msleep(1000);

    disable_servo(port);
    printf("  Done with %s.\n", name);
}

void test_sensors() {
    printf("\nAnalog sensor readings (range 0-4095):\n");
    printf("  EXPECTED: Values change when you cover or move the sensor.\n");
    if (ANALOG_0 >= 0) printf("  Analog port %d: %4d\n", ANALOG_0, analog(ANALOG_0));
    if (ANALOG_1 >= 0) printf("  Analog port %d: %4d\n", ANALOG_1, analog(ANALOG_1));
    if (ANALOG_2 >= 0) printf("  Analog port %d: %4d\n", ANALOG_2, analog(ANALOG_2));
    if (ANALOG_3 >= 0) printf("  Analog port %d: %4d\n", ANALOG_3, analog(ANALOG_3));

    printf("\nDigital sensor readings (0 or 1):\n");
    printf("  EXPECTED: Value flips when you press/activate the sensor.\n");
    if (DIGITAL_0 >= 0) printf("  Digital port %d: %d\n", DIGITAL_0, digital(DIGITAL_0));
    if (DIGITAL_1 >= 0) printf("  Digital port %d: %d\n", DIGITAL_1, digital(DIGITAL_1));
    if (DIGITAL_2 >= 0) printf("  Digital port %d: %d\n", DIGITAL_2, digital(DIGITAL_2));
    if (DIGITAL_3 >= 0) printf("  Digital port %d: %d\n", DIGITAL_3, digital(DIGITAL_3));

    printf("\nWatching digital sensors for 5 seconds - press them now!\n");
    int i;
    for (i = 0; i < 50; i++) {
        if (DIGITAL_0 >= 0 && digital(DIGITAL_0)) printf("  >> Digital port %d TRIGGERED\n", DIGITAL_0);
        if (DIGITAL_1 >= 0 && digital(DIGITAL_1)) printf("  >> Digital port %d TRIGGERED\n", DIGITAL_1);
        if (DIGITAL_2 >= 0 && digital(DIGITAL_2)) printf("  >> Digital port %d TRIGGERED\n", DIGITAL_2);
        if (DIGITAL_3 >= 0 && digital(DIGITAL_3)) printf("  >> Digital port %d TRIGGERED\n", DIGITAL_3);
        msleep(100);
    }
}

// ============================================================
// --- MAIN ---
// ============================================================

int main() {
    enable_servos();

    printf("========================================\n");
    printf("  BOTBALL SELF-TEST: Mecanum Robot\n");
    printf("========================================\n");
    printf("Put the robot on the floor with room\n");
    printf("to move in all directions.\n");
    printf("Press A button to step through each test.\n");
    printf("Check the Console window for output.\n");

    msleep(1000);

#if TEST_MOTORS
    section_header("INDIVIDUAL MOTOR TESTS");
    printf("Each wheel tested alone.\n");
    printf("Watch which wheel spins - it should match the label.\n");
    test_single_motor(MOTOR_LF, "Left Front  (port 0)");
    test_single_motor(MOTOR_RF, "Right Front (port 1)");
    test_single_motor(MOTOR_LB, "Left Back   (port 2)");
    test_single_motor(MOTOR_RB, "Right Back  (port 3)");
    if (EXTRA_MOTOR_A >= 0) test_single_motor(EXTRA_MOTOR_A, "Extra Motor A");
    if (EXTRA_MOTOR_B >= 0) test_single_motor(EXTRA_MOTOR_B, "Extra Motor B");

    section_header("DRIVE DIRECTION TESTS");
    printf("If strafe produces spinning, a motor direction is wrong.\n");
    printf("Use the individual tests above to find which motor.\n");
    test_drive_directions();
#endif

#if TEST_SERVOS
    section_header("SERVO TESTS");
    test_servo(SERVO_0, "Servo 0");
    test_servo(SERVO_1, "Servo 1");
    test_servo(SERVO_2, "Servo 2");
    test_servo(SERVO_3, "Servo 3");
#endif

#if TEST_SENSORS
    section_header("SENSOR TESTS");
    test_sensors();
#endif

    disable_servos();

    printf("\n========================================\n");
    printf("  SELF-TEST COMPLETE\n");
    printf("\n");
    printf("  If everything moved correctly,\n");
    printf("  your hardware is ready.\n");
    printf("\n");
    printf("  Strafe wrong? Check motor directions\n");
    printf("  in the student coding guide.\n");
    printf("  Fix wiring before fixing in code.\n");
    printf("========================================\n");

    ao();
    return 0;
}