#pragma once

#include <array>
#include <chrono>

#include "common.hpp"

template<typename T, int length = 10>
class InterpolatedProperty {
    using Clock = std::chrono::high_resolution_clock;

    std::vector<T> m_buffer;
    std::vector<Clock::time_point> m_time_buffer;

    uint32_t m_head;
    uint32_t m_size;

public:
    InterpolatedProperty(T initial_value) :
        m_buffer(length),
        m_time_buffer(length),
        m_size(0), m_head(0) {
        for(int i = 0; i < length; i++) {
            m_buffer[i] = initial_value;
            m_time_buffer[i] = Clock::now();
        }
    }

    GET_REF(m_buffer, values);
    GET_REF(m_time_buffer, times);

    /** How many slots hold something that was pushed. */
    inline uint32_t size() const {
        return m_size;
    }

    void push(Clock::time_point point, T value) {
        // m_buffer[m_head] = std::move(value);
        // m_time_buffer[m_head] = point;
        //
        // m_head = (m_head + 1) % m_buffer.size();
        //
        // if (m_size < m_buffer.size())
        // {
        //     ++m_size;
        // }
        if(point < m_time_buffer.at(0)) {
            return;
        }

        int i;
        for(i = 1; i < length; i++) {
            if(m_time_buffer.at(i) > point) {
                m_time_buffer[i - 1] = point;
                m_buffer[i - 1] = value;
                return;
            } else {
                m_time_buffer[i - 1] = m_time_buffer[i];
                m_buffer[i - 1] = m_buffer[i];
            }
        }

        m_time_buffer[i - 1] = point;
        m_buffer[i - 1] = value;
        return;

        // for(int i = 0; i < length; i++) {
        //     if(m_time_buffer.at(i) > point) {
        //         for(int o = 1; o < i; o++) {
        //             m_time_buffer[o - 1] = m_time_buffer[o];
        //             m_buffer[o - 1] = m_buffer[o];
        //         }
        //
        //         m_time_buffer[i] = point;
        //         m_buffer[i] = value;
        //
        //         return;
        //     }
        // }
        //
        // m_time_buffer[length] = point;
        // m_buffer[length] = value;

    }

    inline std::tuple<T, T, float> get_values_around(Clock::time_point point) const {
        /* const std::size_t capacity = m_buffer.size();
        const std::size_t oldest = (m_head + capacity - m_size) % capacity;

        if (m_size == 0)
        {
            return { T(), T(), 0.0f };
        }

        // Asking for a time the buffer no longer reaches back to. Standing on
        // the oldest is wrong by however much was missed, but it is wrong in
        // the direction the values were heading, unlike the newest.
        if (point < m_time_buffer[oldest])
        {
            return { m_buffer[oldest], m_buffer[oldest], 0.0f };
        }

        for (std::size_t i = 0; i + 1 < m_size; ++i)
        {
            const std::size_t idx0 = (oldest + i) % capacity;
            const std::size_t idx1 = (oldest + i + 1) % capacity;

            const auto t0 = m_time_buffer[idx0];
            const auto t1 = m_time_buffer[idx1];

            if (point >= t0 && point <= t1)
            {
                const auto total =
                    std::chrono::duration<float>(t1 - t0).count();
                const auto elapsed =
                    std::chrono::duration<float>(point - t0).count();

                const float alpha = total > 0.0f ? elapsed / total : 0.0f;

                return { m_buffer[idx0], m_buffer[idx1], alpha };
            }
        }

        // Handle exact last sample (or clamp)
        const std::size_t last = (oldest + m_size - 1) % capacity;
        return { m_buffer[last], m_buffer[last], 0.0f }; */
        T prev = m_buffer.at(0);
        Clock::time_point prev_point = m_time_buffer.at(0);

        for(int i = 0; i < length; i++) {
            if(m_time_buffer.at(i) > point) {
                float value;
                if(i == 0) {
                    value = 1.0f;
                } else {
                    value = (float)(point - prev_point).count() / (m_time_buffer[i] - prev_point).count();
                }

                return std::make_tuple(prev, m_buffer.at(i), value);
            }

            prev = m_buffer[i];
            prev_point = m_time_buffer[i];
        }

        return std::make_tuple(m_buffer[m_buffer.size() - 1], m_buffer[m_buffer.size() - 1], 1.0f);
    }
};
