physics_native_test.c

#define MM_NATIVE_TEST 1
#include "app.c"
#include <stdio.h>

__attribute__((ms_abi)) LPVOID VirtualAlloc(LPVOID address, SIZE_T size,
                                             DWORD allocation_type, DWORD protection) {
    static unsigned char storage[MAX_TRAJECTORY_SAMPLES * sizeof(TrajectorySample)];
    (void)address; (void)size; (void)allocation_type; (void)protection;
    return size <= sizeof(storage) ? storage : 0;
}

__attribute__((ms_abi)) BOOL VirtualFree(LPVOID address, SIZE_T size, DWORD free_type) {
    (void)address; (void)size; (void)free_type;
    return TRUE;
}

static void default_parameters(SolverParams *p, u32 n, u32 cell_mm) {
    memset(p, 0, sizeof(*p));
    p->n = n; p->cell_mm = cell_mm;
    p->straight_v = 3500u; p->diagonal_v = 3000u; p->turn_v = 1800u;
    p->accel = 12000u; p->brake = 14000u; p->jerk = 120000u;
    p->mu_milli = 900u; p->mu_y_milli = 1050u; p->slip_milli = 80u;
    p->battery_mv = 8400u; p->sag_mv_s = 90u; p->battery_r_mohm = 45u;
    p->battery_capacity_mah = 350u; p->mass_g = 100u;
    p->chassis_length_mm = 90u; p->track_mm = 72u; p->wheel_radius_mm = 12u;
    p->wheel_mass_g = 5u; p->cg_height_mm = 15u; p->motor_r_mohm = 320u;
    p->motor_kt_mnm_a = 18u; p->motor_ke_mv_rad = 2u;
    p->current_limit_ma = 6000u; p->controller_kp_mv_rad = 40u;
    p->turn_radius_mm = 45u; p->max_run = 8u; p->dt_us = 500u;
    u64 lateral_square = ((u64)p->mu_milli * 9810ull * p->turn_radius_mm) / 1000ull;
    u32 lateral_limit = (u32)isqrt_u64(lateral_square);
    if (lateral_limit && p->turn_v > lateral_limit) p->turn_v = lateral_limit;
}

static int solve_route_cpu(AppState *app) {
    SolverParams *p = &app->params;
    u32 node_count = p->n * p->n * 8u;
    for (u32 i = 0; i < node_count; ++i) g_dist[i] = INF64;
    g_dist[0] = 0ull;
    u32 iteration = 0;
    for (; iteration < node_count + 8u; ++iteration) {
        int changed = 0;
        for (u32 source = 0; source < node_count; ++source) {
            u64 pack = g_dist[source];
            if (pack == INF64) continue;
            u64 source_time = pack >> PACK_SHIFT;
            for (u32 action = 0; action < 15u; ++action) {
                if (action < 8u && action >= p->max_run) continue;
                u32 target = 0u;
                u64 edge_cost = 0ull;
                if (!evaluate_action(source, action, source_time, p, &app->maze,
                                     &target, &edge_cost)) continue;
                u64 new_time = source_time + edge_cost;
                if (new_time >= (1ull << 43)) continue;
                u64 candidate = (new_time << PACK_SHIFT) |
                                ((u64)source << 6u) | (u64)action;
                if (candidate < g_dist[target]) {
                    g_dist[target] = candidate;
                    changed = 1;
                }
            }
        }
        if (!changed) break;
    }
    if (iteration >= node_count + 8u) return 0;

    u64 best_pack = INF64;
    u32 best_node = 0u;
    for (u32 y = app->maze.goal_y0; y <= app->maze.goal_y1; ++y) {
        for (u32 x = app->maze.goal_x0; x <= app->maze.goal_x1; ++x) {
            u32 cell = y * p->n + x;
            for (u32 h = 0; h < 8u; ++h) {
                u32 node = (cell << 3u) | h;
                if (g_dist[node] < best_pack) {
                    best_pack = g_dist[node];
                    best_node = node;
                }
            }
        }
    }
    if (best_pack == INF64) return 0;

    u32 trace_count = 0u;
    u32 node = best_node;
    while (node && trace_count + 1u < MAX_ROUTE) {
        u64 pack = g_dist[node];
        if (pack == INF64) return 0;
        g_trace_nodes[trace_count] = node;
        g_trace_actions[trace_count] = (u8)(pack & 63ull);
        node = (u32)((pack & LOW_MASK) >> 6u);
        ++trace_count;
    }
    if (node) return 0;
    app->path_nodes[0] = 0u;
    for (u32 i = 0; i < trace_count; ++i) {
        app->path_nodes[i + 1u] = g_trace_nodes[trace_count - 1u - i];
        app->path_actions[i] = g_trace_actions[trace_count - 1u - i];
    }
    app->path_count = trace_count + 1u;
    app->graph_time_us = best_pack >> PACK_SHIFT;
    app->solve_iterations = iteration + 1u;
    return 1;
}

static int find_stratified_valid_candidate(AppState *app, const DynamicsParams *p,
                                           u32 *candidate_out, DynamicsResult *result_out) {
    static const u32 level[8] = {0u, 9u, 18u, 27u, 36u, 45u, 54u, 63u};
    DynamicsResult best;
    DynamicsResult last;
    memset(&best, 0, sizeof(best));
    memset(&last, 0, sizeof(last));
    u32 best_candidate = 0u;
    u32 failure_counts[DYN_FAIL_COUNT];
    memset(failure_counts, 0, sizeof(failure_counts));
#if defined(MM_EXHAUSTIVE_DIAG)
    float minimum_lateral_failure = 1000000.0f;
    u32 minimum_lateral_candidate = 0u;
    DynamicsResult minimum_lateral_result;
    memset(&minimum_lateral_result, 0, sizeof(minimum_lateral_result));
#endif
#if defined(MM_EXHAUSTIVE_DIAG)
    u32 sample_count = DYNAMICS_CANDIDATES;
#else
    u32 sample_count = 64u;
#endif
    for (u32 sample = 0; sample < sample_count; ++sample) {
#if defined(MM_EXHAUSTIVE_DIAG)
        u32 candidate = sample;
#else
        u32 candidate = level[sample & 7u] | (level[(sample >> 3u) & 7u] << 6u);
#endif
        DynamicsResult current;
        if (simulate_dynamics_cpu(app, p, candidate, 0, &current) &&
            (!best.valid || current.time_s < best.time_s)) {
            best = current;
            best_candidate = candidate;
        }
        if (!current.valid && current.failure_reason < DYN_FAIL_COUNT)
            ++failure_counts[current.failure_reason];
#if defined(MM_EXHAUSTIVE_DIAG)
        if (!current.valid && current.failure_reason == DYN_FAIL_LATERAL_ENVELOPE &&
            current.max_lateral < minimum_lateral_failure) {
            minimum_lateral_failure = current.max_lateral;
            minimum_lateral_candidate = candidate;
            minimum_lateral_result = current;
        }
#endif
        last = current;
    }
    if (!best.valid) {
#if defined(MM_EXHAUSTIVE_DIAG)
        fprintf(stderr, "grid failure counts:");
        for (u32 reason = 1u; reason < DYN_FAIL_COUNT; ++reason)
            if (failure_counts[reason]) fprintf(stderr, " r%u=%u", reason, failure_counts[reason]);
        fprintf(stderr, " minlat_candidate=%u minlat=%.6f reason=%u action=%u time=%.3f\n",
                minimum_lateral_candidate, minimum_lateral_failure,
                minimum_lateral_result.failure_reason,
                minimum_lateral_result.failure_action,
                minimum_lateral_result.time_s);
#endif
        if (result_out) *result_out = last;
        return 0;
    }
    if (candidate_out) *candidate_out = best_candidate;
    if (result_out) *result_out = best;
    return 1;
}

int main(void) {
    Maze maze, previous;
    for (u32 i = 0; i < OFFICIAL_MAZE_COUNT; ++i) {
        if (!decode_official_maze(&maze, i) || !validate_maze(&maze)) {
            fprintf(stderr, "official archive validation failed at %u\n", i);
            return 10;
        }
    }
    for (u32 profile = 0; profile < 2u; ++profile) {
        u32 n = profile ? 32u : 16u;
        u32 cell_mm = profile ? 90u : 180u;
        for (u64 seed = 1u; seed <= 128u; ++seed) {
            if (!generate_maze(&maze, n, cell_mm, seed) || !validate_maze(&maze)) {
                fprintf(stderr, "random maze validation failed profile=%u seed=%llu\n",
                        profile, (unsigned long long)seed);
                return 11;
            }
            if (seed > 1u) {
                int same = 1;
                for (u32 cell = 0; cell < n * n; ++cell)
                    if (maze.walls[cell] != previous.walls[cell]) { same = 0; break; }
                if (same) {
                    fprintf(stderr, "two consecutive seeds produced one layout\n");
                    return 12;
                }
            }
            previous = maze;
        }
    }

    memset(&g_app, 0, sizeof(g_app));
    g_app.maze.n = 16u;
    g_app.maze.cell_mm = 180u;
    g_app.path_count = 7u;
    g_app.path_nodes[0] = ((0u * 16u + 0u) << 3u) | 0u;
    g_app.path_nodes[1] = ((1u * 16u + 0u) << 3u) | 0u;
    g_app.path_nodes[2] = ((2u * 16u + 0u) << 3u) | 0u;
    g_app.path_nodes[3] = ((3u * 16u + 1u) << 3u) | 1u;
    g_app.path_nodes[4] = ((4u * 16u + 2u) << 3u) | 1u;
    g_app.path_nodes[5] = ((4u * 16u + 3u) << 3u) | 2u;
    g_app.path_nodes[6] = ((4u * 16u + 6u) << 3u) | 2u;
    g_app.path_actions[0] = 0u;
    g_app.path_actions[1] = 0u;
    g_app.path_actions[2] = 9u;
    g_app.path_actions[3] = 0u;
    g_app.path_actions[4] = 9u;
    g_app.path_actions[5] = 2u;

    default_parameters(&g_app.params, 16u, 180u);
    DynamicsParams dynamics;
    build_dynamics_params(&g_app.params, &dynamics);
    DynamicsResult best;
    memset(&best, 0, sizeof(best));
    u32 best_candidate = 0;
#if defined(MM_FAST_TEST)
    find_stratified_valid_candidate(&g_app, &dynamics, &best_candidate, &best);
#else
    for (u32 candidate = 0; candidate < DYNAMICS_CANDIDATES; ++candidate) {
        DynamicsResult current;
        simulate_dynamics_cpu(&g_app, &dynamics, candidate, 0, &current);
        if (current.valid && (!best.valid || current.time_s < best.time_s)) {
            best = current;
            best_candidate = candidate;
        }
    }
#endif
    printf("valid=%u candidate=%u time=%.6f slip=%.6f voltage=%.6f lateral=%.6f heading=%.6f position=%.6f cone=%u steps=%u reason=%u action=%u\n",
           best.valid, best_candidate, best.time_s, best.max_slip, best.final_voltage,
           best.max_lateral, best.final_heading_error, best.final_position_error,
           best.cone_hits, best.steps, best.failure_reason, best.failure_action);
    if (!best.valid) {
#if defined(MM_FAST_TEST)
        DynamicsResult failed_replay;
        simulate_dynamics_cpu(&g_app, &dynamics, DYNAMICS_CANDIDATES - 1u, 1, &failed_replay);
        if (g_app.trajectory_count) {
            TrajectorySample *last_failed = &g_app.trajectory[g_app.trajectory_count - 1u];
            printf("failed replay samples=%u x=%.6f y=%.6f heading=%.6f speed=%.6f vy=%.6f yaw=%.6f\n",
                   g_app.trajectory_count, last_failed->x_m, last_failed->y_m,
                   last_failed->heading_rad, last_failed->speed_m_s,
                   last_failed->lateral_m_s, last_failed->yaw_rate_rad_s);
            for (u32 si = 0; si < g_app.trajectory_count; si += 80u) {
                TrajectorySample *s = &g_app.trajectory[si];
                printf("sample %u x=%.5f y=%.5f h=%.5f v=%.5f vy=%.5f yaw=%.5f\n",
                       si, s->x_m, s->y_m, s->heading_rad, s->speed_m_s,
                       s->lateral_m_s, s->yaw_rate_rad_s);
            }
        }
#endif
        return 13;
    }
    DynamicsResult replay;
    if (!simulate_dynamics_cpu(&g_app, &dynamics, best_candidate, 1, &replay) ||
        !g_app.trajectory || g_app.trajectory_count < 100u)
        return 14;
    if (g_app.trajectory_count) {
        TrajectorySample *last = &g_app.trajectory[g_app.trajectory_count - 1u];
        float max_vy = 0.0f, max_speed = 0.0f;
        for (u32 i = 0; i < g_app.trajectory_count; ++i) {
            float av = f_abs(g_app.trajectory[i].lateral_m_s);
            if (av > max_vy) max_vy = av;
            if (g_app.trajectory[i].speed_m_s > max_speed) max_speed = g_app.trajectory[i].speed_m_s;
        }
        printf("replay samples=%u x=%.5f y=%.5f psi=%.5f speed=%.5f\n",
               g_app.trajectory_count, last->x_m, last->y_m,
               last->heading_rad, last->speed_m_s);
        printf("max_speed=%.5f max_vy=%.5f\n", max_speed, max_vy);
        if (max_vy <= 0.0001f || replay.final_position_error > 0.35f * dynamics.cell_m)
            return 15;
    }

    u32 official_dynamics_pass = 0u;
    u32 maximum_actions = 0u;
    u32 target_2024_candidate = 0u;
    DynamicsResult target_2024_result;
    memset(&target_2024_result, 0, sizeof(target_2024_result));
    for (u32 i = 0; i < OFFICIAL_MAZE_COUNT; ++i) {
        if (!decode_official_maze(&g_app.maze, i)) return 16;
        default_parameters(&g_app.params, g_app.maze.n, g_app.maze.cell_mm);
        if (!solve_route_cpu(&g_app)) {
            fprintf(stderr, "official graph solve failed at %u\n", i);
            return 17;
        }
        if (g_app.path_count - 1u > maximum_actions) maximum_actions = g_app.path_count - 1u;
        DynamicsParams official_params;
        build_dynamics_params(&g_app.params, &official_params);
        DynamicsResult official_result;
        u32 official_candidate = 0u;
        int physical_pass = simulate_dynamics_cpu(&g_app, &official_params, 0u, 0,
                                                  &official_result);
        if (!physical_pass || i == 255u) {
            physical_pass = find_stratified_valid_candidate(&g_app, &official_params,
                                                            &official_candidate,
                                                            &official_result);
        }
        if (!physical_pass) {
            fprintf(stderr,
                    "official dynamics failed at %u actions=%u reason=%u action=%u time=%.3f lateral=%.5f\n",
                    i, g_app.path_count - 1u, official_result.failure_reason,
                    official_result.failure_action, official_result.time_s,
                    official_result.max_lateral);
            for (u32 a = 0; a + 1u < g_app.path_count; ++a)
                fprintf(stderr, "%u%s", g_app.path_actions[a],
                        a + 2u < g_app.path_count ? "," : "\n");
            return 18;
        }
        if (i == 255u) {
            target_2024_candidate = official_candidate;
            target_2024_result = official_result;
        }
        ++official_dynamics_pass;
    }
    printf("japan2024 half-size candidate=%u time=%.6f lateral=%.6f steps=%u PASS\n",
           target_2024_candidate, target_2024_result.time_s,
           target_2024_result.max_lateral, target_2024_result.steps);

    u32 random_dynamics_pass = 0u;
    u32 random_maximum_actions = 0u;
    for (u32 profile = 0u; profile < 2u; ++profile) {
        u32 n = profile ? 32u : 16u;
        u32 cell_mm = profile ? 90u : 180u;
        for (u64 seed = 1u; seed <= 128u; ++seed) {
            if (!generate_maze(&g_app.maze, n, cell_mm, seed)) return 19;
            default_parameters(&g_app.params, n, cell_mm);
            if (!solve_route_cpu(&g_app)) return 20;
            if (g_app.path_count - 1u > random_maximum_actions)
                random_maximum_actions = g_app.path_count - 1u;
            DynamicsParams random_params;
            DynamicsResult random_result;
            u32 random_candidate = 0u;
            build_dynamics_params(&g_app.params, &random_params);
            int random_pass = simulate_dynamics_cpu(&g_app, &random_params, 0u, 0,
                                                    &random_result);
            if (!random_pass)
                random_pass = find_stratified_valid_candidate(&g_app, &random_params,
                                                               &random_candidate,
                                                               &random_result);
            if (!random_pass) {
                fprintf(stderr,
                        "random dynamics failed profile=%u seed=%llu actions=%u reason=%u action=%u\n",
                        profile, (unsigned long long)seed, g_app.path_count - 1u,
                        random_result.failure_reason, random_result.failure_action);
                return 21;
            }
            ++random_dynamics_pass;
        }
    }

    SolverParams no_diagonal = g_app.params;
    no_diagonal.diagonal_v = 0u;
    g_app.params = no_diagonal;
    if (!solve_route_cpu(&g_app)) return 22;
    for (u32 i = 0; i < g_app.path_count; ++i)
        if (g_app.path_nodes[i] & 1u) return 23;
    u64 pivot_cost = turn_cost_host(4u, 3142u, 0u, &g_app.params);
    u32 pivot_speed = voltage_scaled_host(g_app.params.turn_v, 0u, &g_app.params);
    u64 expected_pivot_cost =
        ((u64)g_app.params.turn_radius_mm * 3142ull * 1000ull + pivot_speed - 1u) /
        pivot_speed;
    expected_pivot_cost =
        (expected_pivot_cost * (1000u + g_app.params.slip_milli) + 999u) / 1000u;
    expected_pivot_cost += (u64)g_app.params.accel * 250000ull /
                           (g_app.params.jerk ? g_app.params.jerk : 1u) + 1ull;
    if (pivot_cost != expected_pivot_cost) return 24;

    printf("archive=%u archive_dynamics=%u random=256 random_dynamics=%u dynamics_grid=4096 trajectory=%u max_actions=%u random_max_actions=%u diagonal_off=PASS u180=PASS PASS\n",
           OFFICIAL_MAZE_COUNT, official_dynamics_pass, random_dynamics_pass,
           g_app.trajectory_count, maximum_actions, random_maximum_actions);
    return 0;
}