#include "pico/stdlib.h"
#include "config.h"
#include "motor.h"
#include "control.h"
#include "tof.h"
#include "planner.h"
#include "comms.h"

int main() {
    motor_init();
    control_init();
    tof_init();
    comms_init();
    planner_init();

    motor_enable(true);

    const uint32_t dt_ms = 1000 / STATUS_HZ;
    absolute_time_t next = make_timeout_time_ms(dt_ms);

    while (true) {
        planner_step();  // autonomous behavior
        comms_poll();    // publish status over USB
        sleep_until(next);
        next = delayed_by_ms(next, dt_ms);
    }
}