#include "route.h"
#include <math.h>

#define DEG_TO_RAD      (M_PI / 180.0)
#define METRES_PER_DEG  111194.93   // mean Earth radius 6371 km; a block is small enough for a flat projection
#define ACCEL_MPS2      1.5         // pulling away from a corner
#define DECEL_MPS2      2.0         // braking ahead of a corner
#define LATERAL_MPS2    2.0         // sideways acceleration a normal driver takes a bend at
#define STRAIGHT_DEG    3.0         // kinks gentler than this are digitising noise, not bends
#define BEND_SPAN_M     15.0        // longest segment counted towards a bend's radius (see route_init)

static const route_point_t *s_pts;
static int    s_n;
static double s_len[ROUTE_MAX_POINTS];          // segment i runs from point i to point (i+1) % n
static double s_bearing_deg[ROUTE_MAX_POINTS];
static double s_corner_mps[ROUTE_MAX_POINTS];   // speed limit at point i, where segment i starts
static double s_cruise_mps;
static int    s_seg;
static double s_pos_m;                          // distance along the current segment

static double segment_speed(int seg, double pos_m)
{
    double v = s_cruise_mps;
    double pull_away = sqrt(s_corner_mps[seg] * s_corner_mps[seg] + 2.0 * ACCEL_MPS2 * pos_m);
    int next = (seg + 1) % s_n;
    double braking = sqrt(s_corner_mps[next] * s_corner_mps[next]
                          + 2.0 * DECEL_MPS2 * (s_len[seg] - pos_m));
    if (pull_away < v) v = pull_away;
    if (braking < v) v = braking;
    return v;
}

int route_init(const route_point_t *points, int n, double cruise_mps, double min_corner_mps)
{
    if (n < 2 || n > ROUTE_MAX_POINTS) {
        return -1;
    }
    s_pts = points;
    s_n = n;
    s_cruise_mps = cruise_mps;

    for (int i = 0; i < n; i++) {
        const route_point_t *a = &points[i];
        const route_point_t *b = &points[(i + 1) % n];
        double mid_lat = (a->latitude_tenmicrodeg + (double)b->latitude_tenmicrodeg) / 2e7;
        double north_m = (b->latitude_tenmicrodeg - a->latitude_tenmicrodeg) / 1e7 * METRES_PER_DEG;
        double east_m = (b->longitude_tenmicrodeg - a->longitude_tenmicrodeg) / 1e7 * METRES_PER_DEG
                        * cos(mid_lat * DEG_TO_RAD);
        s_len[i] = sqrt(north_m * north_m + east_m * east_m);
        if (s_len[i] < 0.01) {
            return -1;
        }
        double bearing = atan2(east_m, north_m) / DEG_TO_RAD;
        s_bearing_deg[i] = bearing < 0 ? bearing + 360.0 : bearing;
    }

    // Speed limit at each point from how tight the bend there is. A polyline bend of angle theta
    // between segments of length L approximates an arc of radius L / theta, and a car takes a
    // radius R at sqrt(a_lat * R). L is capped at BEND_SPAN_M: at a junction the two streets can be
    // hundreds of metres long, but the car still turns within the width of the crossing.
    for (int i = 0; i < n; i++) {
        double turn = fabs(s_bearing_deg[i] - s_bearing_deg[(i + n - 1) % n]);
        if (turn > 180.0) {
            turn = 360.0 - turn;
        }
        double v = cruise_mps;
        if (turn > STRAIGHT_DEG) {
            double span = s_len[(i + n - 1) % n] < s_len[i] ? s_len[(i + n - 1) % n] : s_len[i];
            if (span > BEND_SPAN_M) {
                span = BEND_SPAN_M;
            }
            v = sqrt(LATERAL_MPS2 * span / (turn * DEG_TO_RAD));
        }
        if (v > cruise_mps) v = cruise_mps;
        if (v < min_corner_mps) v = min_corner_mps;
        s_corner_mps[i] = v;
    }

    // A point's limit also has to respect the bends after it (the car must be able to brake for
    // them within the segments in between) and before it (it can only have sped up so much since).
    // segment_speed only looks at the two ends of a segment, so settle this here. Limits only ever
    // go down, so repeating the two passes until nothing changes terminates.
    for (int changed = 1; changed;) {
        changed = 0;
        for (int k = 0; k < 2 * n; k++) {
            int i = (2 * n - 1 - k) % n;             // backwards: braking
            int next = (i + 1) % n;
            double v = sqrt(s_corner_mps[next] * s_corner_mps[next] + 2.0 * DECEL_MPS2 * s_len[i]);
            if (v < s_corner_mps[i] - 1e-9) { s_corner_mps[i] = v; changed = 1; }
        }
        for (int k = 0; k < 2 * n; k++) {
            int i = k % n;                           // forwards: accelerating
            int next = (i + 1) % n;
            double v = sqrt(s_corner_mps[i] * s_corner_mps[i] + 2.0 * ACCEL_MPS2 * s_len[i]);
            if (v < s_corner_mps[next] - 1e-9) { s_corner_mps[next] = v; changed = 1; }
        }
    }

    s_seg = 0;
    s_pos_m = 0.0;
    return 0;
}

void route_step(double dt_s, route_state_t *out)
{
    // Advance with the speed at the start of the step; at 100 ms steps the error is well under a metre.
    double d = segment_speed(s_seg, s_pos_m) * dt_s;
    while (s_pos_m + d >= s_len[s_seg]) {
        d -= s_len[s_seg] - s_pos_m;
        s_seg = (s_seg + 1) % s_n;
        s_pos_m = 0.0;
    }
    s_pos_m += d;

    const route_point_t *a = &s_pts[s_seg];
    const route_point_t *b = &s_pts[(s_seg + 1) % s_n];
    double f = s_pos_m / s_len[s_seg];
    out->latitude_tenmicrodeg = (int32_t)lround(a->latitude_tenmicrodeg
                                + f * (b->latitude_tenmicrodeg - a->latitude_tenmicrodeg));
    out->longitude_tenmicrodeg = (int32_t)lround(a->longitude_tenmicrodeg
                                 + f * (b->longitude_tenmicrodeg - a->longitude_tenmicrodeg));
    out->speed_cm_s = (uint16_t)lround(segment_speed(s_seg, s_pos_m) * 100.0);
    out->heading_ddeg = (uint16_t)(lround(s_bearing_deg[s_seg] * 10.0) % 3600);
    out->segment = s_seg;
}