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

#include <sched.h>

#include <embb/mtapi/mtapi.h>

#include "defines.h"

#include "timing_header.h"
#include "fun.h"

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

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

auto loop_count(int duration) -> long long
{
    if(duration < sizeof(timetable) / sizeof(int)) {
        return timetable[duration];
    }

    float m = ((float)timetable[94] - (float)timetable[4]) / (90.0f);
    float t = ((float)timetable[9] - m * 9);

    return m * (float) duration + t;
}

auto active_wait(int duration) -> void
{
    for(int i = 0; i < loop_count(duration); i++) {
        donotoptimize();
    }
}

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 = taskset[task_id].wcet;

    active_wait(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_time_base>(start_time - start);
    benchmark[task_id][task_num - 1].end = duration_cast<cpp_time_base>(end_time - start);
    benchmark[task_id][task_num - 1].core_id = core_id;
}

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

/* Make place to store the arguments for any running task instance. */
std::vector<std::pair<int,int>> task_arguments[taskset_length];

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();

    /* Initialize deadlines for every task */
    embb::mtapi::ExecutionPolicy deadline_policy[taskset_length];
    for(int i = 0; i < taskset_length; i++) {
        node.CreateAction(ACTION_ID + i + 1, IdleTask);
        deadline_policy[i] = embb::mtapi::ExecutionPolicy(embb_time_base(taskset[i].deadline));
    }

    using namespace std::chrono;
    start = base_clock::now();

    std::cerr << "Starting TaskStarter thread at: ";
    std::cerr << base_clock::to_time_t(start) << std::endl;

    auto cur_time = duration_cast<cpp_time_base>(base_clock::now() - start);

    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) {
                /* 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. 
                 */
                task_arguments[i][count - 1] = std::make_pair(i,count);
                auto job = node.GetJob(ACTION_ID + i + 1, DOMAIN_ID);

                /* Detached TaskAttribute so we don't have to wait for task completion. */
                embb::mtapi::TaskAttributes detached_attribute;
                detached_attribute.SetDetached(true);
                detached_attribute.SetPolicy(deadline_policy[i]);

                int tmp; auto t = node.Start(job, &task_arguments[i][count - 1], &tmp, detached_attribute);

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

        active_wait(min.count());
        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. By preallocating storage for
     * every instance of every task, we don't need any synchornization.
     */
    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);
        task_arguments[i] = std::vector<std::pair<int,int>>(job_count);
    }

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

    /* Start task loop */
    TaskStarter();

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

    return 0; 
}