#define _GNU_SOURCE
#include <sched.h>
#include <stdio.h>
#include <iostream>
#include <chrono>
#include <cmath>
#include <unistd.h>
void setSchedulingPolicy (int policy, int priority)
{
    sched_param sched;
    sched_getparam(0, &sched);
    sched.sched_priority = priority;
    if (sched_setscheduler(0, policy, &sched)) {
        perror("sched_setscheduler");
        exit(EXIT_FAILURE);
    }
}

void workload_1ms (void)
{
        int repeat = 100000; // tune this for the right amount of workload
        for (int i = 0; i <= repeat; i++)
        {
          int count[1000]={};
          for(int j=0;j<1000;j++)
          {
            count[j]=j;
          }
          //for(int k=0;k<99;k++)
          {
            for(int p=0;p<99;p++)
            {
            if(count[p+1]>count[p])
            {
              int tmp=count[p];
              count[p]=count[p+1];
              count[p+1]=count[p];
            }
            }
          }
          }
}

void pinCPU (int cpu_number)
{
    cpu_set_t mask;
    CPU_ZERO(&mask);

    CPU_SET(cpu_number, &mask);

    if (sched_setaffinity(0, sizeof(cpu_set_t), &mask) == -1)
    {
        perror("sched_setaffinity");
        exit(EXIT_FAILURE);
    }
}

int main (void)
{
    pinCPU (0);
    int period = 7000; // unit: microsecond
    int delta;
    setSchedulingPolicy (SCHED_FIFO, 98);
    while (1)
    {
        std::chrono::system_clock::time_point startTime = std::chrono::system_clock::now();
        for (int j = 0; j < 4; j++)
        {
            workload_1ms ();
        }
        std::chrono::system_clock::time_point endTime = std::chrono::system_clock::now();
        delta = std::chrono::duration_cast<std::chrono::microseconds>(endTime - startTime).count();
        if (delta > period)
        {
            continue;
        }
        else
        {
            usleep (period-delta);
        }
    }
    return 0;
}