#include <iostream>
#include <thread>
#include <vector>

#include <sched.h>

#include <embb/mtapi/mtapi.h>

#include "defines.h"

#define UNUSED(x) ((void)(x))

#define DOMAIN_ID 1
#define NODE_ID 1
#define ACTION_ID 2

auto gcd(long long a, long long b) -> long long
{
    return b == 0 ? a : gcd(b, a % b);
}

auto lcm(long long a, long long b) -> long long
{
    return (a * b) / gcd(a, b);
}

auto calculate_hyperperiod() -> long long 
{
    long long hyperperiod = taskset[0].period;
    for(int i = 1; i < taskset_length; i++) {
        hyperperiod = lcm(hyperperiod, taskset[i].period); 
    }
    return hyperperiod;
}

struct timestamps {
    cpp_time_base start;
    cpp_time_base end;
    int core_id = 0;
};

/**
 * Make place to store timestamps of any running task instance.
 */

base_clock::time_point start;
base_clock::time_point end;
std::vector<timestamps> benchmark[taskset_length];

void benchmark_out()
{
    using namespace std::chrono;

    std::cout << "benchmark results ";
    std::cout << "start " << base_clock::to_time_t(start) << " ";
    std::cout << "end " << base_clock::to_time_t(end) << std::endl;

    for(int i = 0; i < taskset_length; i++) {
        std::cout << "task " << i << " ";
        std::cout << "wcet " << taskset[i].wcet << " ";
        std::cout << "period " << taskset[i].period << " ";
        std::cout << "deadline " << taskset[i].deadline << " ";
        std::cout << "executions " << taskset[i].count << std::endl;

        for(int j = 0; j < benchmark[i].size(); j++) {
            auto task_start = benchmark[i][j].start.count();
            auto task_end = benchmark[i][j].end.count();
            auto core_id = benchmark[i][j].core_id;
            std::cout << "instance " << j << " ";
            std::cout << "start " << task_start << " ";
            std::cout << "end " << task_end << " ";
            std::cout << "core_id " << core_id << std::endl;
        }
    }
}

/******
 * Task Declarations
 ******/

/* Workaround helper function, the base node action is not initialized if the 
 * node gets attributes set.
 */
static void ActionFunction(
        const void* args,
        mtapi_size_t /*args_size*/,
        void* /*result_buffer*/,
        mtapi_size_t /*result_buffer_size*/,
        const void* /*node_local_data*/,
        mtapi_size_t /*node_local_data_size*/,
        mtapi_task_context_t * context) {
    embb::mtapi::TaskContext task_context(context);
    embb::base::Function<void, embb::mtapi::TaskContext &> * func =
        reinterpret_cast<embb::base::Function<void, embb::mtapi::TaskContext &>*>(
                const_cast<void*>(args));
    (*func)(task_context);
    embb::base::Allocation::Delete(func);
}

static void IdleTask(const void* args, mtapi_size_t, void*, mtapi_size_t, 
                     const void*, mtapi_size_t, mtapi_task_context_t*)
{
    using namespace std::chrono;
    /* Get access to parameter data. */
    auto data = static_cast<const std::pair<int,int>*>(args);

    auto task_id = data->first;
    auto task_num = data->second;
    auto start_time = base_clock::now();

    /* idle until task completion. */
    auto idle_time = cpp_time_base(taskset[task_id].wcet);
    std::this_thread::sleep_for(idle_time);

    /* Store our benchmarking data. */
    auto end_time = base_clock::now();
    int core_id = sched_getcpu();

    benchmark[task_id][task_num - 1].start = duration_cast<cpp_base_time>(start_time - start);
    benchmark[task_id][task_num - 1].end = duration_cast<cpp_base_time>(end_time - start);
    benchmark[task_id][task_num - 1].core_id = core_id;
}

/****
 * Main loop of task starter core.
 ****/

static void TaskStarter()
{
    /* Initialize task starter */
    auto& node = embb::mtapi::Node::GetInstance();
    /* Storage for any task which is started. */
    std::vector<embb::mtapi::Task> running;

    auto hyperperiod = calculate_hyperperiod();

    /* Storage for task parameters. */
    int task_cnt = 0;
    for(int i = 0; i < taskset_length; i++) {
        node.CreateAction(ACTION_ID + i + 1, IdleTask);
        task_cnt += hyperperiod / taskset[i].period;
        std::cout << "Task " << i << " wcet: " << taskset[i].wcet << " period: " << taskset[i].period << " deadline: " << taskset[i].deadline << std::endl;
    }
    std::vector<std::pair<int,int>> running_num(task_cnt);
    int task_ptr = 0;

    using namespace std::chrono;

    start = base_clock::now();

    auto cur_time = duration_cast<cpp_time_base>(base_clock::now() - start);
    std::cerr << "Starting TaskStarter thread at: ";
    std::cerr << base_clock::to_time_t(start) << std::endl;

    while(cur_time.count() < hyperperiod) {
        auto min = cpp_time_base::max();

        /* Check for every task if it has to be executed. */
        for(int i = 0; i < taskset_length; i++) {
            auto task_period = cpp_time_base(taskset[i].period);
            /* Execute always then when a next period has started, meaning when
             * cur_time / task_period is bigger than for the last check. The
             * first task_period is to let the tasks start at time 0 and not at
             * time task_period.
             */
            auto count = (cur_time + task_period) / task_period;

            /* Check how long to sleep next, either min(cur_time % period), or 
             * period if cur_time % period == 0
             */
            auto remaining = cur_time % task_period;
            remaining = remaining.count() == 0 ? task_period : remaining;
            if(remaining < min) {
                min = remaining;
            }

            if(count > taskset[i].count) {
                embb::mtapi::ExecutionPolicy deadline_policy(embb_time_base(taskset[i].deadline));
                /* Store parameters for execution.
                 * The count may change during the execution, therefore we have
                 * to make sure that all possible running tasks can access their
                 * parameters. 
                 */
                running_num[task_ptr++] = std::make_pair(i,count);
                auto job = node.GetJob(ACTION_ID + i + 1, DOMAIN_ID);

                int tmp; auto t = node.Start(job, &running_num[task_ptr - 1], &tmp);

                /* Store task to wait for it. */
                running.push_back(t);
                taskset[i].count = count;
            }
        }

        auto wait_start = base_clock::now();
        while(min.count() > 0 && running.size() > 0) {
            auto status = running.front().Wait(min.count()); // TODO factor for anything bigger than milliseconds
            if(status == MTAPI_SUCCESS) {
                running.erase(running.begin());
            }
            auto remain = duration_cast<cpp_time_base>(wait_start - base_clock::now());
            if(remain.count() > 0) {
                min -= remain;
            }
        }
        std::this_thread::sleep_for(min);
        cur_time = duration_cast<cpp_time_base>(base_clock::now() - start);
    }

    /* Wait for all started tasks to be completed. */
    for(auto& task : running) {
        task.Wait(MTAPI_INFINITE);
    }

    end = base_clock::now();
    std::cerr << "Finishing TaskStarter thread at: ";
    std::cerr << base_clock::to_time_t(end) << std::endl;
}

int main(int argc, char* argv[]) 
{
    UNUSED(argc);
    UNUSED(argv);

    /* Initialize node and set global edf as scheduling method. */
    embb::mtapi::NodeAttributes attr;
    attr.SetSchedulerMode(GLOBAL_EDF);
    embb::mtapi::Node::Initialize(DOMAIN_ID, NODE_ID, attr);
    auto& node = embb::mtapi::Node::GetInstance();

    /* Initialize storage for benchmarking data */
    auto hyperperiod = calculate_hyperperiod();
    for(int i = 0; i < taskset_length; i++) {
        auto job_count = hyperperiod / taskset[i].period;
        benchmark[i] = std::vector<timestamps>(job_count);
    }

    /* Workaround, the base node action is not initialized if the node gets
     * attributes set.
     */
    node.CreateAction(ACTION_ID, ActionFunction);

    /* Start task loop */
    TaskStarter();

    /* Print experiment results. */
    benchmark_out();

    return 0; 
}