#include <iostream>
#include <stdlib.h>
#include <pthread.h>
#include <sched.h>
#include <semaphore.h>
#include <time.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/neutrino.h>

#define LOOP_ITERATIONS 10000000
#define TWO_THREADS() THREADS
#define USE_MUTEX()   MUTEX

sem_t thread1sem;
sem_t thread2sem;

#if USE_MUTEX()
pthread_mutex_t thread1mutexa[16];
#define thread1mutex thread1mutexa[0]
#define thread2mutex thread1mutexa[8]

#endif

unsigned long counter[16];
#define counter1 counter[0]
#define counter2 counter[8]
#if 0
unsigned long counter1;
unsigned long counter2;
#endif
unsigned long long startTime1;
unsigned long long endTime1;
unsigned long long startTime2;
unsigned long long endTime2;

extern "C" {
void * thread1(void*);

long long timespec_to_nanosecond(struct timespec const *ts)
{
    long long nanosecond = ts->tv_sec;
    nanosecond *= 1000000000;
    nanosecond += ts->tv_nsec;
    return nanosecond;
}

void * thread1(void *arg)
{
    struct timespec currentTime;
    
#if TWO_THREADS()
	int runmask = 4;
ThreadCtl(_NTO_TCTL_RUNMASK, (void*)runmask);
#endif
    sem_wait(&thread1sem);  // Wait on signal from parent.
    
    sleep(1);  // Sleep to make sure this thread doesn't
               // finish before the parent has a chance
               // to create the other thread(s)
    
    clock_gettime(CLOCK_REALTIME, &currentTime);
    startTime1 = timespec_to_nanosecond(&currentTime);
    
    for(unsigned long long i = 0; i < LOOP_ITERATIONS; ++i);
    {
#if USE_MUTEX()
        //pthread_mutex_lock(&thread1mutex);
	thread1mutex.count++;
#endif
        ++counter1;
#if USE_MUTEX()
	thread1mutex.count--;
       // pthread_mutex_unlock(&thread1mutex);
#endif

#if TWO_THREADS() == 0

#if USE_MUTEX()
        pthread_mutex_lock(&thread2mutex);
#endif
        ++counter2;
#if USE_MUTEX()
        pthread_mutex_unlock(&thread2mutex);
#endif

#endif        
    }

    clock_gettime(CLOCK_REALTIME, &currentTime);
    endTime1 = timespec_to_nanosecond(&currentTime);

}

#if TWO_THREADS()
void mylock(pthread_mutex_t *mutex, int owner) {
	if(_mutex_smp_xchg(&mutex->owner, owner) == 0) {
	      			++mutex->count;
	      }
	  }
void * thread2(void *arg);
void * thread2(void *arg)
{
    struct timespec currentTime;
   	int runmask = 8;
	int owner;

ThreadCtl(_NTO_TCTL_RUNMASK, (void*)runmask);

    sem_wait(&thread2sem);  // Wait on signal from parent.

    sleep(1);  // Sleep to make sure this thread doesn't
               // finish before the parent has a chance
               // to create the other thread(s)

    clock_gettime(CLOCK_REALTIME, &currentTime);
    startTime2 = timespec_to_nanosecond(&currentTime);

	owner = __tls()->owner;
    for(unsigned long long i = 0; i < LOOP_ITERATIONS; ++i)
    {
#if USE_MUTEX()
        //pthread_mutex_lock(&thread2mutex);
	mylock(&thread2mutex, owner);
	//thread2mutex.count++;        
#endif
        ++counter2;
#if USE_MUTEX()
        pthread_mutex_unlock(&thread2mutex);
	//thread2mutex.count--;
#endif
    }

    clock_gettime(CLOCK_REALTIME, &currentTime);
    endTime2 = timespec_to_nanosecond(&currentTime);

}
#endif
}

int main()
{
#if (USE_MUTEX() == 0)
#if (TWO_THREADS() == 0)
    std::cout << "One Thread, no protection" << std::endl;
#else
    std::cout << "Two Threads, no protection" << std::endl;
#endif
#else
#if (TWO_THREADS() == 0)
    std::cout << "One Thread, mutexes" << std::endl;
#else
    std::cout << "Two Threads, mutexes" << std::endl;
#endif
#endif
   	int runmask = 1;
ThreadCtl(_NTO_TCTL_RUNMASK, (void*)runmask);
	sleep(2);
    
    sem_init(&thread1sem,0,0);
    sem_init(&thread2sem,0,0);
#if USE_MUTEX()
    pthread_mutex_init(&thread1mutex, 0);
    pthread_mutex_init(&thread2mutex, 0);
#endif

    pthread_t tid1;
    pthread_attr_t attr1;
    pthread_attr_init(&attr1);
    struct sched_param param1;
    param1.sched_priority = sched_get_priority_max(SCHED_RR);
    pthread_create(&tid1, &attr1, &thread1, NULL);
    pthread_setschedparam(tid1, SCHED_RR, &param1);

#if TWO_THREADS()
    pthread_t tid2;
    pthread_attr_t attr2;
    pthread_attr_init(&attr2);
    struct sched_param param2;
    param2.sched_priority = sched_get_priority_max(SCHED_RR);
    pthread_create(&tid2, &attr2, &thread2, NULL);
    pthread_setschedparam(tid2, SCHED_RR, &param2);
#endif
    
    // timed section
    sem_post(&thread1sem);
    sem_post(&thread2sem);
    pthread_join(tid1, NULL);
#if TWO_THREADS()    
    pthread_join(tid2, NULL);
#endif    
    // end timed section

//    sleep(10);

#if TWO_THREADS()    
    printf("startTime1 0x%llx startTime2 0x%llx\n", startTime1, startTime2);
    printf("endTime1   0x%llx endTime2   0x%llx\n", endTime1, endTime2);
#if USE_MUTEX()
    printf("thread1mutex %p thread2mutex %p\n", &thread1mutex, &thread2mutex);
#endif
    unsigned long long startTime = std::min(startTime1, startTime2);
    unsigned long long endTime = std::max(endTime1, endTime2);
#else
    unsigned long long startTime = startTime1;
    unsigned long long endTime = endTime1;
#endif    
    printf("Total time: %20llu ns\n", (endTime - startTime));
    return 0;
}