Appendix: Commented C Code Listings

/**
 * Nearest Neighbor (NN) path optimization
 * Greedily selects the closest unvisited waypoint at each step
 * Time complexity: O(n²) where n is the number of waypoints
 * 
 * @param points: Array of waypoint coordinates
 * @param count: Number of waypoints to optimize
 * @param start: Starting position (robot's current location)
 * @param route: Output array storing the optimized route indices
 */
static void compute_nearest_neighbor_route(const waypoint_point_t *points, int count,
                                           waypoint_point_t start, int *route) {
    bool visited[MAX_WAYPOINTS] = {0};  // Track which waypoints have been visited
    waypoint_point_t current = start;  // Start from robot's current position

    // Build route step by step
    for (int step = 0; step < count; step++) {
        float best_dist = FLT_MAX;  // Initialize to maximum distance
        int best_idx = -1;  // Index of closest unvisited waypoint
        
        // Find the closest unvisited waypoint
        for (int idx = 0; idx < count; idx++) {
            if (visited[idx]) {
                continue;  // Skip already visited waypoints
            }
            const float dist = waypoint_distance(current, points[idx]);
            if (dist < best_dist) {
                best_dist = dist;
                best_idx = idx;  // Update closest waypoint
            }
        }
        
        if (best_idx < 0) {
            break;  // No more unvisited waypoints
        }
        
        route[step] = best_idx;  // Add to route
        visited[best_idx] = true;  // Mark as visited
        current = points[best_idx];  // Move to next position
    }
}

/**
 * 2-Opt optimization: Improves route by swapping edge pairs
 * Iteratively finds and reverses route segments that reduce total distance
 * Continues until no further improvements can be made
 * 
 * @param points: Array of waypoint coordinates
 * @param count: Number of waypoints
 * @param start: Starting position
 * @param route: Route array (modified in-place)
 */
static void two_opt_local_improvement(const waypoint_point_t *points, int count,
                                      waypoint_point_t start, int *route) {
    if (count < 3) {
        return;  // Need at least 3 points for 2-opt to make sense
    }

    bool improved = true;
    // Keep iterating until no improvements are found
    while (improved) {
        improved = false;
        
        // Try all pairs of edges (i, i+1) and (j, j+1)
        for (int i = 0; i < count - 1; i++) {
            // Get the two endpoints of first edge
            const waypoint_point_t A = (i == 0) ? start : points[route[i - 1]];
            const waypoint_point_t B = points[route[i]];
            
            for (int j = i + 1; j < count; j++) {
                // Get the two endpoints of second edge
                const waypoint_point_t C = points[route[j]];
                const waypoint_point_t D = (j == count - 1) ? start : points[route[j + 1]];
                
                // Calculate current distance: A->B + C->D
                const float current = waypoint_distance(A, B) + waypoint_distance(C, D);
                // Calculate swapped distance: A->C + B->D
                const float swapped = waypoint_distance(A, C) + waypoint_distance(B, D);
                
                // If swapping reduces distance (with small tolerance), reverse the segment
                if (swapped + 1e-3f < current) {
                    reverse_route_segment(route, i, j);  // Reverse route[i..j]
                    improved = true;  // Found improvement, continue searching
                }
            }
        }
    }
}

/**
 * Calculate Euclidean distance from robot to target waypoint
 */
static float distance_to_waypoint(const pose_t *robot_pose, const waypoint_t *target)
{
    float dx = target->x - robot_pose->x;
    float dy = target->y - robot_pose->y;
    return sqrtf(dx * dx + dy * dy);
}

/**
 * Update robot heading from BNO055 IMU Euler angles
 * Reads yaw angle in degrees, converts to radians for internal use
 */
static void update_robot_heading_from_imu(void)
{
    float yaw_deg = fix2float15(euler[0]);   // heading in degrees (15.16 fixed point)
    robot.heading_rad = deg2rad(yaw_deg);    // store in radians
}

/**
 * Calculate absolute angle (in world frame) from robot to target waypoint
 * Returns angle in radians, range [-π, π]
 */
static float angle_to_waypoint(const pose_t *robot_pose, const waypoint_t *target)
{
    float dx = target->x - robot_pose->x;
    float dy = target->y - robot_pose->y;
    float waypoint_angle = atan2f(dy, dx);  // atan2 handles all quadrants correctly
    return waypoint_angle;   // radians
}

/**
 * Calculate signed turn angle error from current heading to waypoint
 * Normalizes error to [-π, π] range for shortest rotation direction
 * Positive error = turn right (clockwise), negative = turn left (counter-clockwise)
 */
static float turn_angle_to_waypoint(const pose_t *robot_pose, const waypoint_t *target)
{
    float desired = angle_to_waypoint(robot_pose, target);  // Desired heading
    float err     = desired - robot_pose->heading_rad;  // Heading error

    // Normalize to [-π, π] to ensure shortest rotation
    while (err >=  3.14159265f) err -= 2.0f * 3.14159265f;
    while (err < -3.14159265f) err += 2.0f * 3.14159265f;

    return err;
}

/**
 * Main movement control thread using protothreads
 * Implements a state machine with three states:
 *   - MOVE_WAITING_FOR_ROUTE: No waypoints available, car is stationary
 *   - MOVE_TURNING: Rotating to face the next waypoint
 *   - MOVE_DRIVING: Moving forward toward the waypoint
 * 
 * Handles dynamic waypoint addition: new waypoints can be added while car is moving
 */
static PT_THREAD (protothread_I_like_to_move_it_move_it(struct pt *pt))
{
    PT_BEGIN(pt);
    static enum {
        MOVE_WAITING_FOR_ROUTE = 0,
        MOVE_TURNING,
        MOVE_DRIVING
    } move_state = MOVE_WAITING_FOR_ROUTE;
    static uint64_t drive_end_time_us = 0;  // Timestamp when forward motion should stop
    static waypoint_t *current_target = NULL;
    static bool route_initialized = false;
    static int last_seen_waypoint_count = -1;  // Track waypoint count to detect new waypoints
    static int last_processed_waypoint_index = -1;  // Track progress through waypoints

    while (1) {
        uint32_t delay_us = 50000;  // Default 50ms delay (20 Hz control loop)

        // Check for new waypoint data
        // Format: [0]=waypoint_count, [1]=robot_x, [2]=robot_y, [3]=wp1_x, [4]=wp1_y, ...
        if (data_array[0] != -1 && data_array[1] != -1 && data_array[2] != -1) {
            int current_waypoint_count = data_array[0];
            
            // Only process if waypoint count increased (new waypoints added)
            if (current_waypoint_count > last_seen_waypoint_count) {
                if (!route_initialized) {
                    // First time: initialize robot position and load all waypoints
                    populate_robot();  // Set robot position from UDP data
                    populate_waypoints_from_index(0);
                    current_waypoint = 1;  // Start from waypoint 1 (skip waypoint 0 = home)
                    last_processed_waypoint_index = 0;
                    route_initialized = (loaded_waypoint_count > 1);
                    move_state = route_initialized ? MOVE_TURNING : MOVE_WAITING_FOR_ROUTE;
                } else {
                    // More waypoints added: load only new waypoints, keep current robot position
                    int start_index = last_processed_waypoint_index + 1;
                    populate_waypoints_from_index(start_index);
                    current_waypoint = start_index;  // Resume from first unvisited waypoint
                    
                    if (current_waypoint < loaded_waypoint_count) {
                        move_state = MOVE_TURNING;  // Resume navigation
                    } else {
                        move_state = MOVE_WAITING_FOR_ROUTE;  // All waypoints completed
                        set_left_motor_signed(0.0f);
                        set_right_motor_signed(0.0f);
                    }
                }
                
                last_seen_waypoint_count = current_waypoint_count;
            }
        }

        // State machine: handle current movement state
        switch (move_state) {
            case MOVE_WAITING_FOR_ROUTE:
                delay_us = 200000;  // Wait 200ms when no route available
                break;

            case MOVE_TURNING: {
                update_robot_heading_from_imu();  // Read current heading from IMU
                float err = turn_angle_to_waypoint(&robot, current_target);  // Calculate heading error
                
                if (fabsf(err) < ANGLE_TOL_RAD) {  // Within tolerance (10 degrees)
                    set_left_motor_signed(0.0f);  // Stop turning
                    set_right_motor_signed(0.0f);
                    
                    float dist = distance_to_waypoint(&robot, current_target);
                    if (dist < 0.01f) {  // Already at waypoint (within 1cm)
                        robot.x = current_target->x;  // Update robot position
                        robot.y = current_target->y;
                        last_processed_waypoint_index = current_waypoint;
                        current_waypoint++;  // Move to next waypoint
                        break;
                    }
                    
                    // Calculate drive time based on distance and speed
                    float time_s = dist / UNITS_PER_SECOND;  // UNITS_PER_SECOND = 50 units/second
                    drive_end_time_us = time_us_64() + (uint64_t)(time_s * 1000000.0f);
                    set_left_motor_signed(PWM_FORWARD);  // Start forward motion
                    set_right_motor_signed(PWM_FORWARD);
                    move_state = MOVE_DRIVING;  // Transition to driving state
                } else if (err < 0.0f) {
                    // Turn left: left motor backward, right motor forward
                    set_left_motor_signed(-TURN_PWM);
                    set_right_motor_signed(+TURN_PWM);
                } else {
                    // Turn right: left motor forward, right motor backward
                    set_left_motor_signed(+TURN_PWM);
                    set_right_motor_signed(-TURN_PWM);
                }
                delay_us = 50000;  // 50ms control loop during turning
                break;
            }

            case MOVE_DRIVING: {
                if (time_us_64() >= drive_end_time_us) {  // Time-based motion complete
                    set_left_motor_signed(0.0f);  // Stop motors
                    set_right_motor_signed(0.0f);
                    robot.x = current_target->x;  // Update position to waypoint
                    robot.y = current_target->y;
                    last_processed_waypoint_index = current_waypoint;
                    current_waypoint++;  // Move to next waypoint
                    move_state = MOVE_TURNING;  // Transition back to turning
                } else {
                    // Continue driving forward
                    set_left_motor_signed(PWM_FORWARD);
                    set_right_motor_signed(PWM_FORWARD);
                }
                delay_us = 50000;
                break;
            }
        }

        PT_YIELD_usec(delay_us);  // Yield control, resume after delay
    }

    PT_END(pt);
}

/**
 * Set left motor speed with signed value
 * Positive = forward, negative = backward, zero = stop
 * Uses H-bridge with separate forward/reverse PWM channels
 */
void set_left_motor_signed(float speed) {
    if (speed > 0.0f) {
        // Forward: use forward PWM channel (GPIO16)
        left_forward_pin_pwm(speed);
        left_reverse_pin_pwm(0.0f);
    } else if (speed < 0.0f) {
        // Backward: use reverse PWM channel (GPIO7)
        left_forward_pin_pwm(0.0f);
        left_reverse_pin_pwm(-speed);  // Negate to get positive PWM value
    } else {
        // Stop: set both channels to zero
        left_forward_pin_pwm(0.0f);
        left_reverse_pin_pwm(0.0f);
    }
}

/**
 * PWM interrupt service routine
 * Called periodically by hardware PWM timer
 * Reads IMU Euler angles for heading estimation
 * NOTE: This runs in an interrupt context - keep it fast!
 */
void on_pwm_wrap() {
    // Clear the interrupt flag
    pwm_clear_irq(slice_fwd);
    pwm_clear_irq(slice_rev);

    // Read IMU Euler angles (yaw/pitch/roll)
    // Data is in 15.16 fixed point format
    // euler[0] = yaw (heading) in degrees
    bno055_read_euler(euler);
}

/**
 * UDP receive callback function
 * Called by lwIP stack when a UDP packet arrives
 * This runs in an interrupt context - MUST be kept short!
 * 
 * @param arg: User argument (unused)
 * @param upcb: UDP protocol control block
 * @param p: Packet buffer containing received data
 * @param addr: Source IP address
 * @param port: Source port
 */
static void
udpecho_raw_recv(void *arg, struct udp_pcb *upcb, struct pbuf *p,
                 const ip_addr_t *addr, u16_t port)
{
    LWIP_UNUSED_ARG(arg);

    if (p != NULL) {
        // Copy packet payload to receive buffer
        // Format: [0]=waypoint_count, [1]=robot_x, [2]=robot_y, [3]=wp1_x, [4]=wp1_y, ...
        memcpy(recv_data, p->payload, UDP_MSG_LEN_MAX);
        
        // Signal protothread that new data is available
        // PT_SEM_SIGNAL can be called from ISR context
        PT_SEM_SIGNAL(pt, &new_udp_recv_s);
        
        // Free the packet buffer (required by lwIP)
        pbuf_free(p);
    }
}

/**
 * Initialize UDP receive socket
 * Binds to UDP_PORT (4444) and sets up receive callback
 */
void 
udpecho_raw_init(void)
{
    udpecho_raw_pcb = udp_new_ip_type(IPADDR_TYPE_ANY);  // Create UDP PCB
    p = pbuf_alloc(PBUF_TRANSPORT, UDP_MSG_LEN_MAX+1, PBUF_RAM);

    if (udpecho_raw_pcb != NULL) {
        err_t err;
        // Bind to local IP address and UDP_PORT
        err = udp_bind(udpecho_raw_pcb, netif_ip4_addr(netif_list), UDP_PORT);

        if (err == ERR_OK) {
            // Register receive callback
            udp_recv(udpecho_raw_pcb, udpecho_raw_recv, NULL);
        }
    }
}

// Motor control constants
#define PWM_FORWARD        4000.0f   // PWM duty cycle for forward motion
#define TURN_PWM           3500.0f   // PWM level used while turning
#define ANGLE_TOL_DEG      10.0f      // Acceptable heading error (degrees)
#define ANGLE_TOL_RAD      (ANGLE_TOL_DEG * 3.14159265f / 180.0f)  // Convert to radians
#define UNITS_PER_SECOND  50.0f     // Robot speed: 50 units/second

// UDP configuration
#define UDP_PORT           4444      // UDP port for communication
#define UDP_MSG_LEN_MAX   1024      // Maximum UDP packet size
#define MAX_WAYPOINTS      256       // Maximum number of waypoints
#define MAX_DATA_SIZE     512       // Maximum data array size

// Robot pose structure
typedef struct {
    float x;              // X position (units)
    float y;              // Y position (units)
    float heading_rad;    // Heading angle (radians)
} pose_t;

// Waypoint structure
typedef struct {
    float x;              // X coordinate
    float y;              // Y coordinate
} waypoint_t;