/*
 * Copyright (C) 2019  Max Regan
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#pragma once

#include "macros.h"

#include "TaskScheduler.h"
#include "SystemTime.h"
#include "Drivers/LowPower.h"
#include "Drivers/RtcDriver.h"

namespace Common {
namespace Schedule {

template <uint32_t MAX_TASKS>
class LowPowerTaskScheduler final : public TaskScheduler {
public:
    LowPowerTaskScheduler() :
        m_tasks(),
        m_task_count(0),
        m_cycle_count(0)
    {}

    [[noreturn]] void run() override
    {
        while (1) {
            cycle();
        }
    }

    void add_task(Task &task, const NextTime &time) override
    {
        if (m_task_count == MAX_TASKS || time.get_type() == ScheduleType::NEVER) {
            return;
        }

        // If the task is already in the task list, don't add, but update
        for (size_t i = 0; i < m_task_count; i++) {
            TaskEvent &event = m_tasks[i];
            if (event.m_task == &task) {
                // Task is already in the list
                if (time < event.m_time) {
                    // Provided time is sooner than the existing time. Update.
                    event.m_time = time;
                }
                return;
            }
        }

        m_tasks[m_task_count++] = TaskEvent(task, time);
    }

    // ~LowPowerTaskScheduler() {}

private:
    struct TaskEvent {

        TaskEvent() :
            m_task(nullptr),
            m_time()
        {}

        TaskEvent(Task &task, NextTime time) :
            m_task(&task),
            m_time(time)
        {}

        Task *m_task;
        NextTime m_time;
    };

    /* FIXME: implement some sort of fixed-size priority queue */
    TaskEvent m_tasks[MAX_TASKS];
    std::size_t m_task_count;
    uint64_t m_cycle_count;

    void inline call_task(TaskEvent &task)
    {
        task.m_time = task.m_task->execute();
    }

    void inline cycle()
    {
        Common::time_t time = 0;
        BSP::SystemTimer::get_time(time);
        bool task_died = false;

        /* Keep state for when the next task will execute. */
        bool execed = false;
        Common::time_t next_time = ~0;

        for (size_t i = 0; i < m_task_count; i++) {
            TaskEvent &event = m_tasks[i];

            if (event.m_time.get_type() == ScheduleType::AT_TIME) {
                if (time >= event.m_time.get_time()) {
                    execed = true;
                    call_task(event);
                } else {
                    next_time = MIN(next_time, event.m_time.get_time());
                }
            } else if (event.m_time.get_type() == ScheduleType::NEVER) {
                task_died = true;
            }
        }

        if (task_died) {
            remove_dead_tasks();
        }

        if (m_task_count == 0) {
            Common::ReturnCode rc = BSP::RtcDriver::set_wakeup_in(Time::seconds(5));
            if (rc == Common::ReturnCode::OK) {
                BSP::LowPower::stop();
            }
        } else if (!execed && (next_time - time > Time::millis(2))) {
            Common::ReturnCode rc = BSP::RtcDriver::set_wakeup_in(next_time - time);
            if (rc == Common::ReturnCode::OK) {
                BSP::LowPower::stop();
            }
        }

        m_cycle_count++;
    }

    void inline remove_dead_tasks()
    {
        std::size_t i_new = 0;
        std::size_t i_old = 0;
        while (i_old < m_task_count) {
            //FIXME: this is broken
            bool is_dead = true;
            if (m_tasks[i_old].m_time.get_type() != ScheduleType::NEVER) {
                is_dead = false;
            }

            if (i_old != i_new) {
                m_tasks[i_new] = m_tasks[i_old];
            }

            if (!is_dead) {
                i_new++;
            }
            i_old++;
        }

        m_task_count = i_new;
    }
};

}
}