#ifndef ROUTE_H
#define ROUTE_H
#include <stdint.h>

// Simulated drive around a closed loop of waypoints, so the beacon looks like a
// car going round the block instead of a parked one.
//
// The car follows straight lines between the points and goes from the last one
// back to the first, forever. For street-following, the points are the street
// geometry from tools/make_route.py (main/route_points.h). Speed is a function
// of where the car is on a segment: it cruises, brakes ahead of each bend down to
// the speed that bend allows, and accelerates away after it. Heading is the
// bearing of the current segment.
//
// Uses only standard headers, so it also compiles on the host for testing.

typedef struct {
    int32_t latitude_tenmicrodeg;   // 1/10 microdegree, same units as the CAM
    int32_t longitude_tenmicrodeg;
} route_point_t;

typedef struct {
    int32_t  latitude_tenmicrodeg;
    int32_t  longitude_tenmicrodeg;
    uint16_t speed_cm_s;            // SpeedValue units (0.01 m/s)
    uint16_t heading_ddeg;          // HeadingValue units (0.1 deg, 0..3599, 0 = north, clockwise)
    int      segment;               // index of the route point the car last passed
} route_state_t;

#define ROUTE_MAX_POINTS 512   // keep in step with MAX_POINTS in tools/make_route.py

// `points` must stay valid for as long as the route is used. Returns 0, or -1
// if n is out of range (2..ROUTE_MAX_POINTS) or a segment has zero length.
// min_corner_mps is the slowest the car ever goes (hairpins, U-turns).
int route_init(const route_point_t *points, int n, double cruise_mps, double min_corner_mps);

// Moves the car on by dt_s seconds and writes its new position into `out`.
void route_step(double dt_s, route_state_t *out);

#endif