#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;
}