#include "track.h"

void Track::split(Real da, Real dl, Real dt) {

    if (points.size() <= 1u) return;

    TrackPointList new_points;

    new_points.push_back(points.front());

    for(TrackPointList::iterator i = points.begin() + 1; i != points.end(); ++i) {

        TrackPoint &prev = *(i - 1);

        int count = 0;

        if (da > 1e-5) {

            int c = (int)ceil(fabs(i->angle - prev.angle)/da);

            if (count < c) count = c;

        }

        if (dl > 1e-5) {

            int c = (int)ceil(fabs(i->length)/dl);

            if (count < c) count = c;

        }

        if (dt > 1e-5) {

            int c = (int)ceil(fabs(i->duration)/dt);

            if (count < c) count = c;

        }

        for(int j = 1; j < count-1; ++j)

            new_points.push_back( TrackPoint::median(j/(Real)count, prev, *i) );

        new_points.push_back(*i);

    }

    points.swap( new_points );

}

void Track::calc_length() {

    if (points.empty()) return;

    points.front().real_pos = TrackPoint::calc_real_pos( points.front() );

    Vector3 prev = points.front().real_pos;

    Real l = 0, time = 0;

    for(TrackPointList::iterator i = points.begin(); i != points.end(); ++i) {

        if (i->speed < 1e-3) i->speed = 1e-3;

        i->real_pos = TrackPoint::calc_real_pos(*i);

        Vector3 d = i->real_pos - prev;

        prev = i->real_pos;

        i->length = d.len();

        l += i->length;

        i->l = l;

        i->duration = i->length/i->speed;

        time += i->duration;

        i->time = time;

    }

}

int Track::index_by_l(Real l) const {

    int i0 = 1, i1 = (int)points.size() - 1;

    if (i1 <= i0) return -1;

    if (l <= points[i0].l) return i0;

    if (points[i1].l <= l) return i1;

    while(i0 + 1 < i1) {

        int i = (i0 + i1)/2;

        (points[i].l <= l ? i0 : i1) = i;

    }

    return i0;

}

int Track::index_by_time(Real t) const {

    int i0 = 1, i1 = (int)points.size() - 1;

    if (i1 <= i0) return -1;

    if (t <= points[i0].time) return i0;

    if (points[i1].time <= t) return i1;

    while(i0 + 1 < i1) {

        int i = (i0 + i1)/2;

        (points[i].time <= t ? i0 : i1) = i;

    }

    return i0;

}