#include "testwait.h"

///Function that returns the cpu time in nanoseconds
inline long long GetCpuTime(void)
{
    clock_gettime(HORLOGE, &orctime);
    return (long long)((&orctime)->tv_sec) *NSEC_PER_SEC + (long long)((&orctime)->tv_nsec);
}

// Trap Ctrl C Interruption
void InterruptC_Handler(void)
{
    printf("Ctrl C Interrupt\n");
    printf ("jittermoy = %ld, jittermax = %ld \n", (long)moy / ji, (long)max);
    printf("sem_post(mainSem); \n");
    sem_post(mainSem);
}

int orcTimerSigMask(void)
{
    sigset_t set/* , oldsig */;
    /* int status; */

    sigfillset(&set);
    if (pthread_sigmask(SIG_BLOCK, &set, NULL) != OK)
    {
        printf("pthread_sigmask Failed %d \n", errno);
        return ERROR;
    }
    sigemptyset(&set);
    sigaddset(&set, SIGINT);
    if (pthread_sigmask(SIG_UNBLOCK, &set, NULL) != OK)
    {
        printf("pthread_sigmask Failed %d \n", errno);
        return ERROR;
    }

    return OK;
}
int orcTimerSigUnMask(void)
{
    sigset_t set/* , oldsig */;
    /* int status; */

    sigfillset(&set);
    if (pthread_sigmask(SIG_BLOCK, &set, NULL) != OK)
    {
        printf("pthread_sigmask Failed %d \n", errno);
        return ERROR;
    }

    sigemptyset(&set);
    sigaddset(&set, TIMER1_SIGNAL);
    if (pthread_sigmask(SIG_UNBLOCK, &set, NULL) != OK)
    {
        printf("pthread_sigmask Failed %d \n", errno);
        return ERROR;
    }
    sigemptyset(&set);
    sigaddset(&set, SIGINT);
    if (pthread_sigmask(SIG_UNBLOCK, &set, NULL) != OK)
    {
        printf("pthread_sigmask Failed %d \n", errno);
        return ERROR;
    }

    return OK;
}

void setTimer(void)
{

    printf("setTimer function\n");
    orcTimerSigMask();
    pthread_sigmask(SIG_SETMASK, NULL, &oldsig);
    if ((status = sigismember(&oldsig, TIMER1_SIGNAL)) == 0) printf("TIMER1_SIGNAL NOT BLOCKED in begin setTimer\n");
    else if (status == 1) printf("TIMER1_SIGNAL is BLOCKED in begin setTimer\n");
    else perror("error in sigismember()");

    printf("waiting mainSem...\n");

        err = sem_wait(mainSem);
        printf("mainSem returns %d  ", err);
        if (err != 0) {perror("mainSem");
	  printf("errno = %d \n", errno);}

    printf("finishing...\n");
    // Cancel the timer
    if (timer_delete(t1) == OK) printf("timer deleted\n");

    return ;

}
void clock_it(void)
{
  int Nsig;
  sigset_t clockset;
  
  orcTimerSigMask();
    sigemptyset(&clockset);
    sigaddset(&clockset,TIMER1_SIGNAL);
  printf("clock-it init\n");
    pthread_sigmask(SIG_SETMASK, NULL, &oldsig);
    status = sigismember(&oldsig, TIMER1_SIGNAL);
    if (status == 0) printf("TIMER1_SIGNAL NOT BLOCKED in clock-it\n");
    else if (status == 1) printf("TIMER1_SIGNAL is BLOCKED in clock-it\n");
    else perror("clock-it init error in sigismember()");

    while (1)
    {
      printf("before sigwait \n");
      sigwait(&clockset,&Nsig); 
      printf("clock_it loop\n");
        now = GetCpuTime();

        if (ji == 0)
        {
            max = moy = drift = 0;
            startime = now;
            ji++;
        }
        else if (ji == 1)
        {
            ji++;startime = now;
        }
        else
        {
            jinc = (now - startime) / 1000;
            jitter = (jinc - (long long) new_setting1.it_interval.tv_nsec / 1000);
            drift += jitter;
            hjitter = abs(jitter);
            moy += hjitter;
            ji++;
            if (hjitter > max) max = hjitter;
#ifdef DEBUG
            printf("increment %ld absjitter %ld drift %ld \n", (long)jinc, (long) hjitter, (long) drift);
#endif
            startime = now;
        }
    }
    return;
}

int main(void)
{
    signal(SIGINT, (void(*)(int))InterruptC_Handler);
    pthread_setcanceltype(PTHREAD_CANCEL_DEFERRED, NULL);
    mysched.sched_priority = sched_get_priority_max(SCHED_FIFO) - 1;
    if ( sched_setscheduler( 0, SCHED_FIFO, &mysched ) == -1 )
    {
        printf("ERROR IN SETTING THE POSIX SCHEDULER\n");
        exit(1);
    }
    mlockall(MCL_CURRENT | MCL_FUTURE);
    orcTimerSigMask();
    pthread_sigmask(SIG_SETMASK, NULL, &oldsig);
    if ((status = sigismember(&oldsig, TIMER1_SIGNAL)) == 0) printf("TIMER1_SIGNAL NOT BLOCKED in main\n");
    else if (status == 1) printf("TIMER1_SIGNAL is BLOCKED in main\n");
    else perror("error in sigismember()");
    printf("RTSIG_MAX = %ld\n", sysconf(_SC_RTSIG_MAX));
    RTPRIO_MIN = sched_get_priority_min(SCHED_FIFO);
    RTPRIO_MAX = sched_get_priority_max(SCHED_FIFO);
    printf("RTPRIO_MIN = %d RTPRIO_MAX = %d\n", RTPRIO_MIN, RTPRIO_MAX);


    mainSem = malloc(sizeof(sem_t));
    if (mainSem == 0)
    {
        printf("ERROR mainSem init \n");
        return 0;
    }
    memset(mainSem, 0, sizeof(sem_t));

    if ((status = sem_init(mainSem, 0, 0)) == OK)
    {
#ifdef DEBUG
        printf("mainSemCreate %p\n", mainSem);
#endif

    }
    else
    {
        printf("mainSemCreate failed\n"); exit(1);
    }

    sig1.sigev_notify = SIGEV_SIGNAL;
    sig1.sigev_signo = TIMER1_SIGNAL;
    sig1.sigev_value.sival_ptr = &t1;

    new_setting1.it_value.tv_sec = 0;
    new_setting1.it_value.tv_nsec = 2000000;
    new_setting1.it_interval.tv_sec = 0;
    new_setting1.it_interval.tv_nsec = 2000000;
#ifdef DEBUG
    printf("TimerSetTime:: new_setting1.it_interval.tv_sec = %ld, new_setting1.it_interval.tv_nsec = %ld \n", new_setting1.it_interval.tv_sec, new_setting1.it_interval.tv_nsec);

    toto = clock_getres(HORLOGE, &clock_resolution);
    printf("Clock resolution is %ld seconds, %ld nanoseconds\n",
           clock_resolution.tv_sec, clock_resolution.tv_nsec);
#endif
    retval = timer_create(HORLOGE, &sig1, &t1);
    if (retval != 0)
    {
        printf("TIMER_CREATE() FAILED\n");
        exit(1);
    }
    else printf("timer created\n");
    if (timer_settime(t1, 0, &new_setting1, NULL) == 0) printf("posix timer runs\n");

    pthread_sigmask(SIG_SETMASK, NULL, &oldsig);
    if ((status = sigismember(&oldsig, TIMER1_SIGNAL)) == 0) printf("TIMER1_SIGNAL NOT BLOCKED in endof setTimer\n");
    else if (status == 1) printf("TIMER1_SIGNAL is BLOCKED in endof setTimer\n");
    else perror("error in sigismember()");

    if (((tid) = (pthread_t*) malloc (sizeof(pthread_t))) == NULL)
    {
        printf("Error malloc orcspawn\n" );
        exit(1);
    }
    if (pthread_create(tid, NULL, (FUNCPTR)clock_it, arg) != 0)
    {
        printf("pthread_create failed\n"); exit(1);
    }
    schedattributes.sched_priority = RTPRIO_MAX;
    pthread_setschedparam(*tid, SCHED_FIFO, &schedattributes);
    pthread_getschedparam(*tid, &pol, &schedattriget);
    prioget = schedattriget.sched_priority;
    printf("thr clock-it created pol %d prio %d Id %p \n", pol, prioget, tid);


    if (pthread_create(&thr_setTimer, NULL, (FUNCPTR)setTimer, NULL) != 0)
    {
        printf("thr_setTimer failed\n"); exit(1);
    }
    schedattributes.sched_priority = RTPRIO_MAX;
    pthread_setschedparam(thr_setTimer, SCHED_FIFO, &schedattributes);
    pthread_getschedparam(thr_setTimer, &pol, &schedattriget);
    prioget = schedattriget.sched_priority;
    printf("thr_setTimer created pol %d prio %d Id %p \n", pol, prioget,&thr_setTimer );
    //waiting...
    pthread_join(thr_setTimer, NULL);
    printf("pthread_join returns\n");

    j = sem_destroy(mainSem);
    if (j == OK)
    {
        free(mainSem); printf("mainSem deleted\n");
    }
    if (pthread_kill(*tid, 0) == OK)
    {
        printf("TaskDelete %p\n", tid);
        status = pthread_cancel(*tid);
    }
    if (status == OK) free(tid);
    return OK;
}