#include <sys/cdefs.h>
#include <sys/param.h>
#include <sys/cpuset.h>
#include <sys/sysctl.h>

#include <machine/cpufunc.h>
#include <machine/cpu.h>

#include <err.h>
#include <pthread.h>
#include <pthread_np.h>
#include <signal.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

//#include "movups.h"

const int N_DATA = 16;
const int N_THREADS = 4;

void
movups(long *data)
{
    for (int i=0; i<N_DATA; i++)
        data[i] = 0;
}

bool use_sse = true;

pthread_mutex_t the_mutex = PTHREAD_MUTEX_INITIALIZER;
volatile bool keep_running = true;
int run_seconds;

typedef long count_t;
#define COUNT_FMT "%ld"

typedef union td_arg {
    struct {
        count_t counter;
        long data[N_DATA];
    } s;
    char pad[CACHE_LINE_SIZE];
} td_arg_t;
td_arg_t counters[N_THREADS];

void *
tfunc(void *_arg)
{
    td_arg_t *arg = _arg;
    int error;

    while (keep_running) {
        error = pthread_mutex_lock(&the_mutex);
        if (error) errc(1, error, "lock");
        error = pthread_mutex_unlock(&the_mutex);
        if (error) errc(1, error, "unlock");
        /*
        if ((arg->s.counter & 0xf) == 0)
            pthread_yield();
        */
        // Use the SSE registers.
        if (use_sse)
            movups(arg->s.data);
        ++arg->s.counter;
    }

    return (NULL);
}

int
main(int argc, char **argv)
{
    int error;
    pthread_t threads[N_THREADS];
    long data[N_DATA];

    if (argc == 1)
        run_seconds = 5;
    if (argc >= 2)
        run_seconds = atoi(argv[1]);
    if (argc >= 3)
        use_sse = atoi(argv[2]) ? true : false;

    //printf("run_seconds = %d\nuse_sse = %d\n", run_seconds, use_sse);

    // Use the SSE registers.
    if (use_sse)
        movups(data);

    error = pthread_mutex_init(&the_mutex, NULL);
    if (error) errc(1, error, "pthread_mutex_init");

    for (int i = 0; i < N_THREADS; ++i) {
        counters[i].s.counter = 0.0;
        error = pthread_create(&threads[i], NULL, tfunc, &counters[i]);
        if (error) errc(1, error, "pthread_create");
    }

    sleep(run_seconds);
    keep_running = false;
    error = pthread_mutex_lock(&the_mutex);
    if (error) errc(1, error, "lock");
    error = pthread_mutex_unlock(&the_mutex);
    if (error) errc(1, error, "unlock");

    count_t sum = 0;
    for (int i = 0; i < N_THREADS; ++i) {
        error = pthread_join(threads[i], NULL);
        if (error) errc(1, error, "pthread_join");
        sum += counters[i].s.counter;
    }

    printf(COUNT_FMT"\n", sum/run_seconds);

    return (0);
}