// ============================================================
// SELF-TEST: Standard (Non-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.
// ============================================================

#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.
// Example: only one extra motor? Set EXTRA_MOTOR_B to -1.
// ============================================================

// Drive motors
#define LEFT_MOTOR      3
#define RIGHT_MOTOR     0

// Extra motors, e.g. arm motor, pulley (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        -1
#define SERVO_3        -1

// 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   1500   // 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");
}

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

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

    printf("\nMotor: %s (port %d)\n", name, port);
    printf("  EXPECTED: Only this motor spins, not the whole robot.\n");
    wait_for_a();

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

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

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

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: Standard Robot\n");
    printf("========================================\n");
    printf("Keep the robot clear of obstacles.\n");
    printf("Watch each component as it's tested.\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("MOTOR TESTS");
    printf("Each motor tested individually.\n");
    test_motor(LEFT_MOTOR,    "Left Motor");
    test_motor(RIGHT_MOTOR,   "Right Motor");
    test_motor(EXTRA_MOTOR_A, "Extra Motor A");
    test_motor(EXTRA_MOTOR_B, "Extra Motor B");
#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("  Something wrong? Check the cable\n");
    printf("  and port number for that component.\n");
    printf("========================================\n");

    ao();
    return 0;
}