/**
 * @file main.c
 * @brief Maze Solver with Integrated Wall Following and Encoder Feedback
 * @author MicroMouse Team
 * @date October 26, 2025
 *
 * Left-hand rule maze solving algorithm with:
 * - Ultrasonic sensors for wall detection and distance control (PID)
 * - Motor encoders for accurate turning and straight-line tracking
 * - Decision making based on wall presence
 *
 * Target: ATmega328P (Arduino Nano compatible)
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h>
#include "config.h"
#include "motors.h"
#include "uart.h"
#include "utils.h"
#include "sensors.h"

// ========== GLOBAL VARIABLES ==========
// Encoder counters
volatile int32_t left_encoder_count = 0;
volatile int32_t right_encoder_count = 0;
static uint8_t last_encoder_state = 0;

// Global sensor data
sensor_data_t sensor_data;

// ========== PID CONTROLLER STRUCTURE ==========
typedef struct
{
    float kp, ki, kd;
    float error, last_error;
    float integral, derivative;
    float output;
    uint32_t last_time;
} pid_controller_t;

// Wall following PID controller
pid_controller_t wall_pid = {
    .kp = PID_KP,
    .ki = PID_KI,
    .kd = PID_KD,
    .error = 0.0f,
    .last_error = 0.0f,
    .integral = 0.0f,
    .derivative = 0.0f,
    .output = 0.0f,
    .last_time = 0};

// ========== WALL FOLLOWING STATE ==========
typedef enum
{
    STATE_BOTH_WALLS,
    STATE_LEFT_WALL,
    STATE_RIGHT_WALL,
    STATE_NO_WALLS,
    STATE_FRONT_WALL
} wall_state_t;

wall_state_t current_state = STATE_NO_WALLS;

// ========== SENSOR SMOOTHING FILTERS ==========
typedef struct
{
    float prev_value;
    float alpha;
} filter_t;

filter_t left_filter = {0.0f, SENSOR_FILTER_ALPHA};
filter_t right_filter = {0.0f, SENSOR_FILTER_ALPHA};
filter_t front_filter = {0.0f, SENSOR_FILTER_ALPHA};

// ========== OPENING DETECTION TIMING ==========
static uint32_t left_opening_detected_time = 0;
static uint32_t right_opening_detected_time = 0;
static uint8_t left_opening_active = 0;
static uint8_t right_opening_active = 0;

// ========== FUNCTION PROTOTYPES ==========
void maze_solve_decision(void);
uint8_t check_maze_decision_needed(void);

// ========== ENCODER FUNCTIONS ==========

/**
 * @brief Initialize encoder pins and interrupts
 */
void encoders_init(void)
{
    // Configure encoder pins as inputs with pull-ups
    LEFT_ENC_A_DDR &= ~(1 << LEFT_ENC_A_BIT);
    LEFT_ENC_A_PORT |= (1 << LEFT_ENC_A_BIT);

    LEFT_ENC_B_DDR &= ~(1 << LEFT_ENC_B_BIT);
    LEFT_ENC_B_PORT |= (1 << LEFT_ENC_B_BIT);

    RIGHT_ENC_A_DDR &= ~(1 << RIGHT_ENC_A_BIT);
    RIGHT_ENC_A_PORT |= (1 << RIGHT_ENC_A_BIT);

    RIGHT_ENC_B_DDR &= ~(1 << RIGHT_ENC_B_BIT);
    RIGHT_ENC_B_PORT |= (1 << RIGHT_ENC_B_BIT);

    // Initialize last state
    last_encoder_state = PINC & ((1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3));

    // Enable Pin Change Interrupts on PCINT1 (for PCINT8-14, Port C)
    PCICR |= (1 << PCIE1);

    // Enable all 4 encoder pins: PCINT8-11 (A0-A3)
    PCMSK1 |= (1 << PCINT8) | (1 << PCINT9) | (1 << PCINT10) | (1 << PCINT11);
}

/**
 * @brief PCINT1 interrupt - Encoder ticks on Port C
 * Uses quadrature encoding for better accuracy and direction detection
 */
ISR(PCINT1_vect)
{
    uint8_t current_state = PINC & ((1 << PC0) | (1 << PC1) | (1 << PC2) | (1 << PC3));

    // Left encoder - A phase (PC0/A0) is primary
    static uint8_t last_left_a = 0;
    uint8_t left_a = (current_state & (1 << PC0)) ? 1 : 0;
    uint8_t left_b = (current_state & (1 << PC1)) ? 1 : 0;

    // Detect rising edge on A phase
    if (left_a && !last_left_a)
    {
        // Check B phase for direction
        // REVERSED: Left motor counts backward when forward
        if (left_b)
        {
            left_encoder_count++; // Swapped: was --, now ++
        }
        else
        {
            left_encoder_count--; // Swapped: was ++, now --
        }
    }
    last_left_a = left_a;

    // Right encoder - A phase (PC2/A2) is primary
    static uint8_t last_right_a = 0;
    uint8_t right_a = (current_state & (1 << PC2)) ? 1 : 0;
    uint8_t right_b = (current_state & (1 << PC3)) ? 1 : 0;

    // Detect rising edge on A phase
    if (right_a && !last_right_a)
    {
        // Check B phase for direction (normal orientation)
        if (right_b)
        {
            right_encoder_count--; // Backward
        }
        else
        {
            right_encoder_count++; // Forward
        }
    }
    last_right_a = right_a;

    last_encoder_state = current_state;
}

/**
 * @brief Get encoder counts safely
 */
void encoders_get_counts(int32_t *left, int32_t *right)
{
    cli();
    *left = left_encoder_count;
    *right = right_encoder_count;
    sei();
}

/**
 * @brief Apply low-pass filter for smooth sensor readings
 */
float apply_filter(filter_t *filter, float new_value)
{
    filter->prev_value = (filter->alpha * new_value) +
                         ((1.0f - filter->alpha) * filter->prev_value);
    return filter->prev_value;
}

/**
 * @brief Constrain value within limits
 */
float constrain_float(float value, float min_val, float max_val)
{
    if (value < min_val)
        return min_val;
    if (value > max_val)
        return max_val;
    return value;
}

int16_t constrain_int(int16_t value, int16_t min_val, int16_t max_val)
{
    if (value < min_val)
        return min_val;
    if (value > max_val)
        return max_val;
    return value;
}

/**
 * @brief Calculate encoder-based correction for straight tracking
 */
float calculate_encoder_correction(int32_t left_ticks, int32_t right_ticks)
{
    int32_t diff = left_ticks - right_ticks;

    // Apply deadband
    if (diff > -ENCODER_DEADBAND && diff < ENCODER_DEADBAND)
    {
        return 0.0f;
    }

    // Calculate proportional correction
    float correction = (float)diff * ENCODER_CORRECTION_GAIN;

    // Constrain correction
    return constrain_float(correction, -ENCODER_MAX_CORRECTION, ENCODER_MAX_CORRECTION);
}

/**
 * @brief Calculate PID correction for wall following
 */
float calculate_wall_pid(float error)
{
    uint32_t current_time = millis();
    float dt = (float)(current_time - wall_pid.last_time) / 1000.0f;
    if (dt < 0.001f)
        dt = 0.001f;
    wall_pid.last_time = current_time;

    wall_pid.error = error;

    // P term
    float p_term = wall_pid.kp * wall_pid.error;

    // I term with anti-windup
    wall_pid.integral += wall_pid.error * dt;
    wall_pid.integral = constrain_float(wall_pid.integral, -INTEGRAL_LIMIT, INTEGRAL_LIMIT);
    float i_term = wall_pid.ki * wall_pid.integral;

    // D term with filtering
    float derivative_raw = (wall_pid.error - wall_pid.last_error) / dt;
    wall_pid.derivative = (DERIVATIVE_FILTER * derivative_raw) +
                          ((1.0f - DERIVATIVE_FILTER) * wall_pid.derivative);
    float d_term = wall_pid.kd * wall_pid.derivative;

    wall_pid.output = p_term + i_term + d_term;
    wall_pid.output = constrain_float(wall_pid.output, -WALL_MAX_CORRECTION, WALL_MAX_CORRECTION);

    wall_pid.last_error = wall_pid.error;

    return wall_pid.output;
}

/**
 * @brief Check if maze decision is needed (left/right opening or front wall)
 * Returns 1 if decision needed, 0 otherwise
 * Implements configurable delay for left/right openings
 */
uint8_t check_maze_decision_needed(void)
{
    uint32_t current_time = millis();

    // Check for left wall opening (priority 1)
    uint8_t left_wall_absent = (sensor_data.left_distance > MAZE_LEFT_THRESHOLD);

    // Check for right wall opening (priority 3)
    uint8_t right_wall_absent = (sensor_data.right_distance > MAZE_RIGHT_THRESHOLD);

    // Check for front wall (triggers decision immediately)
    uint8_t front_wall_present = (sensor_data.front_distance < FRONT_STOP_DISTANCE);

    // Front wall detection - immediate response (no delay)
    if (front_wall_present)
    {
        // Reset opening timers when front wall detected
        left_opening_active = 0;
        right_opening_active = 0;
        return 1;
    }

    // Left opening detection with delay
    if (left_wall_absent)
    {
        if (!left_opening_active)
        {
            // First time detecting left opening - start timer
            left_opening_active = 1;
            left_opening_detected_time = current_time;
        }
        else
        {
            // Check if delay has elapsed
            if ((current_time - left_opening_detected_time) >= MAZE_OPENING_DELAY)
            {
                // Delay complete - trigger decision
                left_opening_active = 0;  // Reset for next time
                right_opening_active = 0; // Reset right as well
                return 1;
            }
        }
    }
    else
    {
        // Left wall reappeared - reset timer
        left_opening_active = 0;
    }

    // Right opening detection with delay (only if left is blocked)
    if (right_wall_absent && !left_wall_absent)
    {
        if (!right_opening_active)
        {
            // First time detecting right opening - start timer
            right_opening_active = 1;
            right_opening_detected_time = current_time;
        }
        else
        {
            // Check if delay has elapsed
            if ((current_time - right_opening_detected_time) >= MAZE_OPENING_DELAY)
            {
                // Delay complete - trigger decision
                right_opening_active = 0; // Reset for next time
                left_opening_active = 0;  // Reset left as well
                return 1;
            }
        }
    }
    else
    {
        // Right wall reappeared - reset timer
        right_opening_active = 0;
    }

    // No decision needed yet
    return 0;
}

/**
 * @brief Determine wall following state
 */
wall_state_t determine_wall_state(void)
{
    uint8_t left_valid = (sensor_data.left_distance >= MIN_WALL_DISTANCE &&
                          sensor_data.left_distance <= MAX_WALL_DISTANCE);
    uint8_t right_valid = (sensor_data.right_distance >= MIN_WALL_DISTANCE &&
                           sensor_data.right_distance <= MAX_WALL_DISTANCE);
    uint8_t front_close = (sensor_data.front_distance < FRONT_STOP_DISTANCE);

    if (front_close)
    {
        return STATE_FRONT_WALL;
    }
    else if (left_valid && right_valid)
    {
        return STATE_BOTH_WALLS;
    }
    else if (left_valid)
    {
        return STATE_LEFT_WALL;
    }
    else if (right_valid)
    {
        return STATE_RIGHT_WALL;
    }
    else
    {
        return STATE_NO_WALLS;
    }
}

/**
 * @brief Integrated control with encoder and wall feedback fusion
 * Smoothly combines both correction signals
 * Also checks for maze decision points (left/right openings or front wall)
 */
void apply_integrated_control(int32_t left_ticks, int32_t right_ticks)
{
    float encoder_correction = 0.0f;
    float wall_correction = 0.0f;
    float total_correction = 0.0f;
    float error = 0.0f;

    // Check if maze decision is needed (left/right opening or front wall)
    if (check_maze_decision_needed())
    {
        maze_solve_decision();
        return;
    }

    // Get encoder-based correction
    encoder_correction = calculate_encoder_correction(left_ticks, right_ticks);

    // Get wall-based correction (if walls detected)
    float left_dist = apply_filter(&left_filter, (float)sensor_data.left_distance);
    float right_dist = apply_filter(&right_filter, (float)sensor_data.right_distance);

    current_state = determine_wall_state();

    switch (current_state)
    {
    case STATE_BOTH_WALLS:
        // Center between walls
        error = left_dist - right_dist;
        wall_correction = calculate_wall_pid(error);
        break;

    case STATE_LEFT_WALL:
        // Follow left wall
        error = left_dist - TARGET_WALL_DISTANCE;
        wall_correction = calculate_wall_pid(error);
        break;

    case STATE_RIGHT_WALL:
        // Follow right wall
        error = TARGET_WALL_DISTANCE - right_dist;
        wall_correction = calculate_wall_pid(error);
        break;

    case STATE_NO_WALLS:
        // No walls - rely on encoders only
        wall_correction = 0.0f;
        wall_pid.integral = 0.0f;
        wall_pid.last_error = 0.0f;
        break;

    case STATE_FRONT_WALL:
        // This should be caught by check_maze_decision_needed() above
        // But handle it here as safety backup
        maze_solve_decision();
        return;
    }

    // Fuse corrections with weighted average for smooth control
    if (current_state == STATE_NO_WALLS)
    {
        // No walls - use encoder correction only
        total_correction = encoder_correction;
    }
    else
    {
        // Walls detected - blend both corrections
        total_correction = (ENCODER_WEIGHT * encoder_correction) +
                           (WALL_WEIGHT * wall_correction);
    }

    // Calculate motor speeds with fused correction
    // CORRECTED: Swap the signs to match physical turning behavior
    // Positive correction = need to steer LEFT = slow left motor, speed up right motor
    // Negative correction = need to steer RIGHT = speed up left motor, slow right motor
    int16_t left_speed = MOTOR_SPEED - (int16_t)total_correction;
    int16_t right_speed = MOTOR_SPEED + (int16_t)total_correction;

    // Constrain to safe range
    left_speed = constrain_int(left_speed, 60, 200);
    right_speed = constrain_int(right_speed, 60, 200);

    // Apply speeds
    motors_set_speed((uint8_t)left_speed, (uint8_t)right_speed);
}

/**
 * @brief Reset encoder counters
 */
void encoders_reset(void)
{
    cli();
    left_encoder_count = 0;
    right_encoder_count = 0;
    sei();
}

/**
 * @brief Turn left 90 degrees using encoder feedback
 * Left motor backward, right motor forward (rotate in place)
 * Matches Arduino test code: Turn Left = Left backward + Right forward
 */
void turn_left_90(void)
{
    uart_println(">>> TURNING LEFT 90° <<<");

    // Reset encoders
    encoders_reset();

    int32_t left_ticks, right_ticks;
    uint32_t avg_ticks = 0;

    // Turn in place: left motor backward, right motor forward
    // Motor A (Left) - Backward
    AIN1_PORT &= ~(1 << AIN1_PIN); // AIN1 = LOW
    AIN2_PORT |= (1 << AIN2_PIN);  // AIN2 = HIGH
    OCR0B = TURN_SPEED;

    // Motor B (Right) - Forward
    BIN1_PORT |= (1 << BIN1_PIN);  // BIN1 = HIGH
    BIN2_PORT &= ~(1 << BIN2_PIN); // BIN2 = LOW
    OCR2B = TURN_SPEED;

    // Turn until target reached
    while (1)
    {
        encoders_get_counts(&left_ticks, &right_ticks);

        // Use absolute values and average both wheels
        int32_t left_abs = (left_ticks < 0) ? -left_ticks : left_ticks;
        int32_t right_abs = (right_ticks < 0) ? -right_ticks : right_ticks;
        avg_ticks = (left_abs + right_abs) / 2;

        if (avg_ticks >= TICKS_90_DEGREE - TURN_TOLERANCE)
        {
            break;
        }

        delay_ms(10);
    }

    // Stop motors
    motors_stop();

    uart_print("Turn complete! Ticks: ");
    uart_print_int(avg_ticks);
    uart_println("");

    delay_ms(300); // Brief pause after turn
}

/**
 * @brief Turn right 90 degrees using encoder feedback
 * Left motor forward, right motor backward (rotate in place)
 * Matches Arduino test code: Turn Right = Left forward + Right backward
 */
void turn_right_90(void)
{
    uart_println(">>> TURNING RIGHT 90° <<<");

    // Reset encoders
    encoders_reset();

    int32_t left_ticks, right_ticks;
    uint32_t avg_ticks = 0;

    // Turn in place: left motor forward, right motor backward
    // Motor A (Left) - Forward
    AIN1_PORT |= (1 << AIN1_PIN);  // AIN1 = HIGH
    AIN2_PORT &= ~(1 << AIN2_PIN); // AIN2 = LOW
    OCR0B = TURN_SPEED;

    // Motor B (Right) - Backward
    BIN1_PORT &= ~(1 << BIN1_PIN); // BIN1 = LOW
    BIN2_PORT |= (1 << BIN2_PIN);  // BIN2 = HIGH
    OCR2B = TURN_SPEED;

    // Turn until target reached
    while (1)
    {
        encoders_get_counts(&left_ticks, &right_ticks);

        // Use absolute values and average both wheels
        int32_t left_abs = (left_ticks < 0) ? -left_ticks : left_ticks;
        int32_t right_abs = (right_ticks < 0) ? -right_ticks : right_ticks;
        avg_ticks = (left_abs + right_abs) / 2;

        if (avg_ticks >= TICKS_90_DEGREE - TURN_TOLERANCE)
        {
            break;
        }

        delay_ms(10);
    }

    // Stop motors
    motors_stop();

    uart_print("Turn complete! Ticks: ");
    uart_print_int(avg_ticks);
    uart_println("");

    delay_ms(300); // Brief pause after turn
}

/**
 * @brief Turn around 180 degrees using encoder feedback
 * Performs two consecutive 90-degree right turns
 * Compensated for actual 180° turn (was only reaching ~160°)
 * Motor directions swapped to match physical orientation
 */
void turn_around_180(void)
{
    uart_println(">>> TURNING AROUND 180° <<<");

    // Reset encoders
    encoders_reset();

    int32_t left_ticks, right_ticks;
    uint32_t avg_ticks = 0;

    // Calculate target for 180° turn from config.h
    uint32_t target_ticks = TICKS_180_DEGREE;

    // Turn in place: left motor forward, right motor backward (same as turn_right_90)
    // Performs a 180° right turn (two consecutive right turns)
    // Motor A (Left) - Forward
    AIN1_PORT |= (1 << AIN1_PIN);  // AIN1 = HIGH
    AIN2_PORT &= ~(1 << AIN2_PIN); // AIN2 = LOW
    OCR0B = TURN_SPEED;

    // Motor B (Right) - Backward
    BIN1_PORT &= ~(1 << BIN1_PIN); // BIN1 = LOW
    BIN2_PORT |= (1 << BIN2_PIN);  // BIN2 = HIGH
    OCR2B = TURN_SPEED;

    // Turn until target reached
    while (1)
    {
        encoders_get_counts(&left_ticks, &right_ticks);

        // Use absolute values and average both wheels
        int32_t left_abs = (left_ticks < 0) ? -left_ticks : left_ticks;
        int32_t right_abs = (right_ticks < 0) ? -right_ticks : right_ticks;
        avg_ticks = (left_abs + right_abs) / 2;

        if (avg_ticks >= target_ticks - TURN_TOLERANCE)
        {
            break;
        }

        delay_ms(10);
    }

    // Stop motors
    motors_stop();

    uart_print("180° Turn complete! Ticks: ");
    uart_print_int(avg_ticks);
    uart_print(" (Target: ");
    uart_print_int(target_ticks);
    uart_println(")");

    delay_ms(300); // Brief pause after turn
}

/**
 * @brief Maze solving decision algorithm (Left-hand rule)
 * Priority: LEFT > FORWARD > RIGHT > TURN AROUND
 *
 * Decision Logic:
 * 1. IF left wall is absent (left sensor > threshold):
 *    → Turn LEFT and move forward
 * 2. ELSE IF front wall is absent (front sensor > threshold):
 *    → Move FORWARD
 * 3. ELSE IF right wall is absent (right sensor > threshold):
 *    → Turn RIGHT and move forward
 * 4. ELSE (all three walls detected - dead end):
 *    → Turn AROUND (180°) and move forward
 */
void maze_solve_decision(void)
{
    uart_println("");
    uart_println("====================================================");
    uart_println("MAZE DECISION POINT");
    uart_println("====================================================");

    // Stop motors for decision making
    motors_stop();
    delay_ms(MAZE_DECISION_DELAY);

    // Read all sensors
    sensors_read_all(&sensor_data);

    // Display sensor readings
    uart_print("Sensors - L:");
    uart_print_int(sensor_data.left_distance);
    uart_print(" F:");
    uart_print_int(sensor_data.front_distance);
    uart_print(" R:");
    uart_print_int(sensor_data.right_distance);
    uart_println("");

    // Check walls using thresholds
    uint8_t left_wall_absent = (sensor_data.left_distance > MAZE_LEFT_THRESHOLD);
    uint8_t front_wall_absent = (sensor_data.front_distance > MAZE_FRONT_THRESHOLD);
    uint8_t right_wall_absent = (sensor_data.right_distance > MAZE_RIGHT_THRESHOLD);

    uart_print("Wall Status - L:");
    uart_print(left_wall_absent ? "OPEN" : "BLOCKED");
    uart_print(" F:");
    uart_print(front_wall_absent ? "OPEN" : "BLOCKED");
    uart_print(" R:");
    uart_print(right_wall_absent ? "OPEN" : "BLOCKED");
    uart_println("");

    // Decision Logic (Left-hand rule priority)
    if (left_wall_absent)
    {
        // Priority 1: Turn LEFT if left wall is absent
        uart_println("Decision: LEFT WALL ABSENT - TURNING LEFT");
        uart_println("====================================================");
        turn_left_90();

        // Move forward briefly after turn
        uart_println("Moving forward after left turn...");
        encoders_reset();
        motors_set_speed(MOTOR_SPEED, MOTOR_SPEED);
        delay_ms(MAZE_FORWARD_TIME);
    }
    else if (front_wall_absent)
    {
        // Priority 2: Move FORWARD if front is clear
        uart_println("Decision: FRONT CLEAR - MOVING FORWARD");
        uart_println("====================================================");

        // Continue moving forward (no turn needed)
        encoders_reset();
        motors_set_speed(MOTOR_SPEED, MOTOR_SPEED);
    }
    else if (right_wall_absent)
    {
        // Priority 3: Turn RIGHT if right wall is absent
        uart_println("Decision: RIGHT WALL ABSENT - TURNING RIGHT");
        uart_println("====================================================");
        turn_right_90();
        ;

        // Move forward briefly after turn
        uart_println("Moving forward after right turn...");
        encoders_reset();
        motors_set_speed(MOTOR_SPEED, MOTOR_SPEED);
        delay_ms(MAZE_FORWARD_TIME);
    }
    else
    {
        // Priority 4: DEAD END - Turn around 180°
        uart_println("Decision: DEAD END - TURNING AROUND 180°");
        uart_println("====================================================");
        turn_around_180();

        // Move forward briefly after turn
        uart_println("Moving forward after 180° turn...");
        encoders_reset();
        motors_set_speed(MOTOR_SPEED, MOTOR_SPEED);
        delay_ms(MAZE_FORWARD_TIME);
    }

    // Reset PID state for fresh start after decision
    wall_pid.integral = 0.0f;
    wall_pid.last_error = 0.0f;
    wall_pid.last_time = millis();

    uart_println("Resuming maze solving...");
    uart_println("");
}

// ========== MAIN PROGRAM ==========

int main(void)
{
    // Disable watchdog
    wdt_disable();

    // Initialize peripherals
    uart_init(BAUD_RATE);
    timer_init();
    sensors_init();
    encoders_init();
    motors_init();
    motors_enable();

    // Enable global interrupts
    sei();

    // Wait for serial port to stabilize
    delay_ms(1000);

    // Print banner
    uart_println("");
    uart_println("====================================================");
    uart_println("  MicroMouse - Maze Solver (Left-Hand Rule)");
    uart_println("  Encoder + Ultrasonic Sensor Fusion");
    uart_println("====================================================");
    uart_println("");
    uart_println("Maze Solving Algorithm:");
    uart_println("  1. LEFT wall absent  -> Turn LEFT");
    uart_println("  2. FRONT clear       -> Move FORWARD");
    uart_println("  3. RIGHT wall absent -> Turn RIGHT");
    uart_println("  4. Dead end          -> Turn AROUND (180°)");
    uart_println("");
    uart_println("Motor Configuration:");
    uart_println("  Motor A (Left):  D7=AIN1, D6=AIN2, D5=PWMA");
    uart_println("  Motor B (Right): D4=BIN1, D2=BIN2, D3=PWMB");
    uart_println("  Standby: D8=STBY (HIGH)");
    uart_println("");
    uart_println("Encoder Configuration:");
    uart_println("  Left Motor:  A0=ENC_A, A1=ENC_B");
    uart_println("  Right Motor: A2=ENC_A, A3=ENC_B");
    uart_println("");
    uart_println("Ultrasonic Sensors:");
    uart_println("  Left:  D9(TRIG), D10(ECHO)");
    uart_println("  Front: A4(TRIG), A5(ECHO)");
    uart_println("  Right: D11(TRIG), D12(ECHO)");
    uart_println("");
    uart_print("Base Speed: ");
    uart_print_int(MOTOR_SPEED);
    uart_println("");
    uart_print("PID Gains - Kp:");
    uart_print_float(PID_KP, 2);
    uart_print(" Ki:");
    uart_print_float(PID_KI, 3);
    uart_print(" Kd:");
    uart_print_float(PID_KD, 2);
    uart_println("");
    uart_print("Fusion - Encoder:");
    uart_print_float(ENCODER_WEIGHT, 1);
    uart_print(" Wall:");
    uart_print_float(WALL_WEIGHT, 1);
    uart_println("");
    uart_println("");
    uart_println("Maze Thresholds:");
    uart_print("  Left: >");
    uart_print_int(MAZE_LEFT_THRESHOLD);
    uart_print("cm  Front: >");
    uart_print_int(MAZE_FRONT_THRESHOLD);
    uart_print("cm  Right: >");
    uart_print_int(MAZE_RIGHT_THRESHOLD);
    uart_println("cm");
    uart_println("");
    uart_println("Turn Configuration:");
    uart_print("  Ticks/Rev: ");
    uart_print_int(TICKS_PER_REVOLUTION);
    uart_println("");
    uart_print("  90° Turn Ticks: ");
    uart_print_int(TICKS_90_DEGREE);
    uart_println("");
    uart_print("  Wheelbase: ");
    uart_print_float(WHEELBASE_CM, 1);
    uart_println(" cm");
    uart_println("====================================================");
    uart_println("");

    uart_println("Starting in 3 seconds...");
    delay_ms(1000);
    uart_println("3...");
    delay_ms(1000);
    uart_println("2...");
    delay_ms(1000);
    uart_println("1...");
    delay_ms(1000);
    uart_println("");
    uart_println(">>> MAZE SOLVER ACTIVE <<<");
    uart_println("");

    // Reset encoder counters
    encoders_reset();

    // Initialize PID timer
    wall_pid.last_time = millis();

    // Start motors
    motors_set_speed(MOTOR_SPEED, MOTOR_SPEED);

    uint32_t last_print = 0;
    uint32_t last_sensor = 0;
    uint32_t last_control = 0;

    // Main integrated control loop
    while (1)
    {
        int32_t left_ticks, right_ticks;
        encoders_get_counts(&left_ticks, &right_ticks);

        // Update sensors at specified rate
        if (millis() - last_sensor > SENSOR_UPDATE_MS)
        {
            sensors_read_all(&sensor_data);
            last_sensor = millis();
        }

        // Apply integrated control at specified rate
        if (millis() - last_control > CONTROL_PERIOD_MS)
        {
            apply_integrated_control(left_ticks, right_ticks);
            last_control = millis();
        }

        // Print status every 500ms
        if (millis() - last_print > 500)
        {
            // Print encoder data
            uart_print("ENC L:");
            uart_print_int(left_ticks);
            uart_print(" R:");
            uart_print_int(right_ticks);
            uart_print(" D:");
            uart_print_int(left_ticks - right_ticks);

            // Print wall data based on state
            uart_print(" | ");
            switch (current_state)
            {
            case STATE_BOTH_WALLS:
                uart_print("BOTH L:");
                uart_print_int(sensor_data.left_distance);
                uart_print(" R:");
                uart_print_int(sensor_data.right_distance);
                break;
            case STATE_LEFT_WALL:
                uart_print("LEFT L:");
                delay_ms(100);
                uart_print_int(sensor_data.left_distance);
                break;
            case STATE_RIGHT_WALL:
                uart_print("RIGHT R:");
                delay_ms(100);
                uart_print_int(sensor_data.right_distance);
                break;
            case STATE_NO_WALLS:
                uart_print("NO_WALLS");
                break;
            case STATE_FRONT_WALL:
                uart_print("FRONT F:");
                uart_print_int(sensor_data.front_distance);
                break;
            }
            uart_println("");

            last_print = millis();
        }

        delay_ms(10);
    }

    return 0;
}