#include <sys/time.h>
#include <stdint.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <signal.h>
#include <time.h>
#include <pthread.h>
#include <math.h>

int64_t curTime( )
{
    timeval cur_tm;
    gettimeofday(&cur_tm,NULL);
    return (int64_t)cur_tm.tv_sec*1000000 + cur_tm.tv_usec;
}

void *Task( void * )
{
    float prec, prec_max = 0;
    long long work_tm, cur_tm;
    int per = 100000;	//us
    struct timespec get_tm;

    printf("TEST Pthread thr: %xh\n",pthread_self());

    //Calc next work time and sleep
    /*cur_tm = work_tm = curTime();
    for(int i = 0; i < 1000; i++)
    {
	work_tm += per;
	get_tm.tv_sec = (work_tm-cur_tm)/1000000; get_tm.tv_nsec = 1000*((work_tm-cur_tm)%1000000);
	printf("TEST 00: sec=%d; nsec=%d\n",get_tm.tv_sec,get_tm.tv_nsec);
	nanosleep(&get_tm,NULL);
	cur_tm = curTime();
	prec = (float)(cur_tm-work_tm)/1e3;
	if(prec > prec_max) prec_max = prec;
	printf("Precission %f ms; max %f\n",prec,prec_max);
    }*/

    clock_gettime(CLOCK_REALTIME,&get_tm);
    work_tm = (long long)get_tm.tv_sec*1000000000+get_tm.tv_nsec;

    for(int i=0; i < 1000; i++)
    {
	work_tm=work_tm+per*1000;
	get_tm.tv_sec = work_tm/1000000000; get_tm.tv_nsec = work_tm%1000000000;
	clock_nanosleep(CLOCK_REALTIME,TIMER_ABSTIME,&get_tm,NULL);
	clock_gettime(CLOCK_REALTIME,&get_tm);
	cur_tm = (long long)get_tm.tv_sec*1000000000+get_tm.tv_nsec;
	prec = (float)(cur_tm-work_tm)/1000000.;
	if(prec > prec_max) prec_max=prec;
	printf("Precission %f ms; max %f\n",prec,prec_max);
    }



}

int main( int argc, char **argv )
{
    pthread_t pthrid;
    pthread_attr_t pthr_attr;
    pthread_attr_init(&pthr_attr);
    pthread_attr_setinheritsched( &pthr_attr, PTHREAD_EXPLICIT_SCHED );
    struct sched_param prior;
    pthread_attr_setschedpolicy( &pthr_attr, SCHED_RR );
    //pthread_attr_setschedpolicy(&pthr_attr,SCHED_OTHER);
    prior.sched_priority=50;
    pthread_attr_setschedparam(&pthr_attr,&prior);
    pthread_create(&pthrid,&pthr_attr,Task,NULL);
    pthread_attr_destroy(&pthr_attr);

    //> Floatpionf test
    double v1 = 2.5;
    double v2 = 3.5;
    printf("TEST sin(Pi) = %f\n",(v1*v2));

    //> Packing test
    struct SCode { uint8_t cod; uint16_t reg; double val; } __attribute__((packed));
    SCode cd;
    cd.val = 3.14159265;
    printf("TEST Pack. Must: 11; Have: %d; Get value: %f\n",sizeof(SCode),cd.val);

    printf("TEST Pthread proc: %xh\n",pthread_self());

    sleep(100);

    return 0;
}