#include <sys/time.h>
#include <unistd.h>
#include <stdio.h>
#include <pthread.h>

pthread_mutex_t mutex1 = PTHREAD_MUTEX_INITIALIZER;
pthread_mutex_t mutex2 = PTHREAD_MUTEX_INITIALIZER;

struct timeval tv_start;
struct timeval tv_stop;

#define halfCountTo 4000000
#define countTo halfCountTo * 2
#define sleepTime 10

int start_time(void)
{
  return gettimeofday(&tv_start, 0);
}

void tv_sub(struct timeval * out, struct timeval *in)
{
  if ((out->tv_usec -= in->tv_usec) < 0)
  {
    --out->tv_sec;
    out->tv_usec += 1000000;
  }

  out->tv_sec -= in->tv_sec;
}

double stop_time(void)
{
  if (gettimeofday(&tv_stop, 0) == -1)
    return 0.0;

  tv_sub(&tv_stop, &tv_start);

  return tv_stop.tv_sec + tv_stop.tv_usec / 1000000.0;
}

void * fn(void * param)
{
  int i = 0;

  while (i < halfCountTo)
  {
//    usleep(sleepTime);
    ++i;
    pthread_yield();
  }
}

int main(int argc, char ** argv)
{
  pthread_attr_t attr;
  pthread_t t1, t2;
  int i = 0;

  double two_thread_time;
  double one_thread_time;
  double total_time;
  double overhead_s;

  /* pthread_mutex_init always returns 0, apparently. */

  (void)pthread_mutex_init(&mutex1, NULL);
  (void)pthread_mutex_init(&mutex2, NULL);

  pthread_mutex_lock(&mutex1);
  pthread_mutex_lock(&mutex2);

  if (0 != pthread_attr_init(&attr))
  {
    perror("pthread_attr_init");
    return 1;
  }

  if (0 != pthread_create(&t1, &attr, fn, 0))
  {
    perror("pthread_create");
    return 1;
  }

  if (0 != pthread_create(&t2, &attr, fn, 0))
  {
    perror("pthread_create");
    return 1;
  }

  start_time();

  pthread_mutex_unlock(&mutex1);
  pthread_mutex_unlock(&mutex2);

  if (0 != pthread_join(t1, 0))
  {
    perror("pthread_join");
    return 1;
  }
 
  if (0 != pthread_join(t2, 0))
  {
    perror("pthread_join");
    return 1;
  }

  two_thread_time = stop_time() * 2;

  start_time();

  while (i < countTo)
  {
//    usleep(sleepTime);
    ++i;
  }

  one_thread_time = stop_time();

  total_time = two_thread_time + one_thread_time;

  overhead_s = (two_thread_time - one_thread_time) / countTo * 1000000;

  printf("Context switching overhead: %0.8f us\n", overhead_s);

  return 0;
}