/*!
  \file
  \brief タイムスタンプ付きの位置管理

  \author Satofumi KAMIMURA

  $Id$
*/

#include "TimestampedPosition.h"
#include "MathUtils.h"
#include <deque>

using namespace qrk;
using namespace std;


namespace
{
    enum {
        ValidPeriodMsec = 3000,     // [msec]
    };

    class TicksPosition
    {
    public:
        Position<long> position;
        long timestamp;


        TicksPosition(const Position<long>& position_, long timestamp_)
            : position(position_), timestamp(timestamp_)
        {
        }
    };
}


struct TimestampedPosition::pImpl
{
    deque<TicksPosition> positions_;
};


TimestampedPosition::TimestampedPosition(void) : pimpl(new pImpl)
{
}


TimestampedPosition::~TimestampedPosition(void)
{
}


void TimestampedPosition::addPosition(const Position<long>& position,
                                      long timestamp)
{
    // !!! 途中の位置のデータも登録できるようにする
    pimpl->positions_.push_back(TicksPosition(position, timestamp));

    long old_timestamp = timestamp - ValidPeriodMsec;
    while (pimpl->positions_.back().timestamp < old_timestamp) {
        pimpl->positions_.pop_front();
    }
}


Position<long> TimestampedPosition::position(long timestamp) const
{
    size_t n = pimpl->positions_.size();
    if (n <= 1) {
        // データ数が少ないときは、最後の位置を返す
        return pimpl->positions_.back().position;
    }

    // 範囲外の時刻については、一番近いときの位置を返す
    deque<TicksPosition>::const_iterator previous = pimpl->positions_.begin();

    if (previous->timestamp > timestamp) {
        return pimpl->positions_.front().position;
    }

    for (deque<TicksPosition>::const_iterator it = pimpl->positions_.begin();
         it != pimpl->positions_.end(); ++it) {
        if (it->timestamp == timestamp) {
            // 指定時刻印と一致した位置を返す
            return it->position;
        }
        if (it->timestamp > timestamp) {
            // 補間した位置を返す
            double ratio = timestamp - previous->timestamp;
            double period = it->timestamp - previous->timestamp;
            double p = 1.0 * ratio / period;
            long x =
                static_cast<long>(previous->position.x +
                                  (it->position.x - previous->position.x) * p);
            long y =
                static_cast<long>(previous->position.y +
                                  (it->position.y - previous->position.y) * p);

            double radian_diff =
                it->position.to_rad() - previous->position.to_rad();
            if (radian_diff > M_PI) {
                radian_diff -= 2.0 * M_PI;
            } else if (radian_diff < -M_PI) {
                radian_diff  += 2.0 * M_PI;
            }
            double radian = previous->position.to_rad() + (radian_diff * p);

            return Position<long>(x, y, rad(radian));
        }
        previous = it;
    }
    return pimpl->positions_.back().position;
}
