#include <tobii/tobii.h>
#include <tobii/tobii_streams.h>
#include <stdio.h>
#include <assert.h>
#include <cstring>
#include <vector>
#include <string>
#include <sstream>
#include <lsl_cpp.h>
#include <sys/types.h> // For uid_t
#include <pwd.h> // For struct passwd and getpwuid()
#include <thread>
#include <atomic>
#include <termios.h>
#include <unistd.h>

//tobii handles
static tobii_api_t *api = nullptr;
static tobii_device_t *device = nullptr;

//LSL outlet streams
static lsl::stream_outlet *gazePointOutlet;
static lsl::stream_outlet *gazeOriginOutlet;
static lsl::stream_outlet *eyePositionNormalizedOutlet;
static lsl::stream_outlet *userPresenceOutlet;

std::atomic<bool> exit_flag(false);

void inputThread()
{
    struct termios oldt, newt;
    tcgetattr(STDIN_FILENO, &oldt);
    newt = oldt;
    newt.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &newt);

    char c;
    read(STDIN_FILENO, &c, 1); // Blocks until a key is pressed
    exit_flag = true;

    tcsetattr(STDIN_FILENO, TCSANOW, &oldt); // Restore original settings
}

//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// tobii callbacks
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void gaze_point_callback(tobii_gaze_point_t const *gaze_point, void *user_data)
{
    (void)user_data; //ignore user_data argument

    std::vector<float> result(2, 0.0);

    if(gaze_point->validity == TOBII_VALIDITY_VALID)
    {
        result[0] = gaze_point->position_xy[0];
        result[1] = gaze_point->position_xy[1];

//        printf("[gaze point]: %f, %f\n",
//               gaze_point->position_xy[0],
//               gaze_point->position_xy[1]);

        if(gazePointOutlet)
        {
            gazePointOutlet->push_sample(result, lsl::local_clock());
        }
    }

}
void gaze_origin_callback(tobii_gaze_origin_t const *gaze_origin, void *user_data)
{
    (void)user_data; //ignore user_data argument

    std::vector<float> result(6, 0.0);

    if(gaze_origin->left_validity == TOBII_VALIDITY_VALID)
    {
        result[0] = gaze_origin->left_xyz[0];
        result[1] = gaze_origin->left_xyz[1];
        result[2] = gaze_origin->left_xyz[2];
    }
    else
    {
        result[0] = -1.0;
        result[1] = -1.0;
        result[2] = -1.0;
    }

    if(gaze_origin->right_validity == TOBII_VALIDITY_VALID)
    {
        result[3] = gaze_origin->right_xyz[0];
        result[4] = gaze_origin->right_xyz[1];
        result[5] = gaze_origin->right_xyz[2];
    }
    else
    {
        result[3] = -1.0;
        result[4] = -1.0;
        result[5] = -1.0;
    }

//    printf("[gaze origin] left: %f, %f, %f\n[gaze origin] right: %f, %f, %f\n",
//           result[0],
//           result[1],
//           result[2],
//           result[3],
//           result[4],
//           result[5]);
    if(gazeOriginOutlet)
    {
        gazeOriginOutlet->push_sample(result, lsl::local_clock());
    }
}
void eyePositionNormalizedCallback(tobii_eye_position_normalized_t const *eyePosition, void *user_data)
{
    (void)user_data; //ignore user_data argument

    std::vector<float> result(6, 0.0);
    if(eyePosition->left_validity == TOBII_VALIDITY_VALID)
    {
        result[0] = eyePosition->left_xyz[0];
        result[1] = eyePosition->left_xyz[1];
        result[2] = eyePosition->left_xyz[2];
    }
    if(eyePosition->right_validity == TOBII_VALIDITY_VALID)
    {
        result[3] = eyePosition->right_xyz[0];
        result[4] = eyePosition->right_xyz[1];
        result[5] = eyePosition->right_xyz[2];
    }
//    printf("[eye position] left: %f, %f, %f\n[eye position] right: %f, %f, %f\n",
//           result[0],
//           result[1],
//           result[2],
//           result[3],
//           result[4],
//           result[5]);
    if(eyePositionNormalizedOutlet)
    {
        eyePositionNormalizedOutlet->push_sample(result, lsl::local_clock());
    }
}

//NOT SUPPORTED :-|
/*
void headPoseCallback(tobii_head_pose_t const *headPose, void *user_data)
{
    (void)user_data; //ignore user_data argument

    static std::vector<float> sample = {0, 0, 0, 0, 0, 0};

    if(headPose->position_validity == TOBII_VALIDITY_VALID)
    {
        sample[0] = headPose->position_xyz[0];
        sample[1] = headPose->position_xyz[1];
        sample[2] = headPose->position_xyz[2];
    }
    if(headPose->rotation_validity_xyz[0] == TOBII_VALIDITY_VALID)
    {
        sample[3] = headPose->rotation_xyz[0];
    }
    if(headPose->rotation_validity_xyz[1] == TOBII_VALIDITY_VALID)
    {
        sample[4] = headPose->rotation_xyz[1];
    }
    if(headPose->rotation_validity_xyz[2] == TOBII_VALIDITY_VALID)
    {
        sample[5] = headPose->rotation_xyz[2];
    }

//    printf("[head pose] left: %f, %f, %f\n[head rotation] right: %f, %f, %f\n",
//           sample[0],
//           sample[1],
//           sample[2],
//           sample[3],
//           sample[4],
//           sample[5]);
}
*/
void userPresenceCallback(tobii_user_presence_status_t status, int64_t timestamp_us, void *user_data)
{
    (void)timestamp_us; //ignore timestamp_us argument
    (void)user_data; //ignore user_data argument

    static std::string msgPrefix("[user presence]");
    if(status == TOBII_USER_PRESENCE_STATUS_PRESENT)
    {
//        printf("%s: present\n", msgPrefix.c_str());
        if(userPresenceOutlet)
        {
            std::string msg("present");
            userPresenceOutlet->push_sample(&msg, lsl::local_clock());
        }
    }
    else if(status == TOBII_USER_PRESENCE_STATUS_AWAY)
    {
//        printf("%s: away\n", msgPrefix.c_str());
        if(userPresenceOutlet)
        {
            std::string msg("away");
            userPresenceOutlet->push_sample(&msg, lsl::local_clock());
        }
    }
    else if(status == TOBII_USER_PRESENCE_STATUS_UNKNOWN)
    {
//        printf("%s: unknown\n", msgPrefix.c_str());
        if(userPresenceOutlet)
        {
            std::string msg("unknown");
            userPresenceOutlet->push_sample(&msg, lsl::local_clock());
        }
    }
}
//--------------------------------------------------------------------

static void url_receiver(char const *url, void *user_data)
{
    char *buffer = (char *)user_data;
    if(*buffer != '\0') // only keep first value
    {
        return;
    }

    if(strlen(url) < 256)
    {
        strcpy(buffer, url);
    }
}

void initTobii()
{
    tobii_error_t error = tobii_api_create(&api, nullptr, nullptr);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_api_create() call error: %s\n", tobii_error_message(error));
    }

    char url[256] = {0x00};
    error = tobii_enumerate_local_device_urls(api, url_receiver, url);
    if((error != TOBII_ERROR_NO_ERROR) && (*url != '\0'))
    {
        printf("tobii_enumerate_local_device_urls() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_device_create(api, url, &device);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_device_create() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_gaze_point_subscribe(device, gaze_point_callback, 0);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_gaze_point_subscribe() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_gaze_origin_subscribe(device, gaze_origin_callback, 0);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_gaze_origin_subscribe() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_eye_position_normalized_subscribe(device, eyePositionNormalizedCallback, 0);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_eye_position_normalized_subscribe() call error: %s\n", tobii_error_message(error));
    }

//NOT SUPPORTED :-|
/*
    error = tobii_head_pose_subscribe(device, headPoseCallback, 0);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_head_pose_subscribe() call error: %s\n", tobii_error_message(error));
    }
*/

    error = tobii_user_presence_subscribe(device, userPresenceCallback, 0);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_user_presence_subscribe() call error: %s\n", tobii_error_message(error));
    }
}

void deinitTobii()
{
    tobii_error_t error = tobii_user_presence_unsubscribe(device);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_user_presence_unsubscribe() call error: %s\n", tobii_error_message(error));
    }

    //NOT SUPPORTED :-|
//    error = tobii_head_pose_unsubscribe(device);
//    if(error != TOBII_ERROR_NO_ERROR)
//    {
//        printf("tobii_head_pose_unsubscribe() call error: %s\n", tobii_error_message(error));
//    }

    error = tobii_eye_position_normalized_unsubscribe(device);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_eye_position_normalized_unsubscribe() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_gaze_origin_unsubscribe(device);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_gaze_origin_unsubscribe() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_gaze_point_unsubscribe(device);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_gaze_point_unsubscribe() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_device_destroy(device);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_device_destroy() call error: %s\n", tobii_error_message(error));
    }

    error = tobii_api_destroy(api);
    if(error != TOBII_ERROR_NO_ERROR)
    {
        printf("tobii_api_destroy() call error: %s\n", tobii_error_message(error));
    }
}

void initLSL()
{
    //get username to use as source_id parameter for created LSL streams
    uid_t uid = getuid(); // Get the real user ID of the calling process
    struct passwd *pw = getpwuid(uid); // Get password structure for the given UID
    if(pw != nullptr)
    {
        printf("Username: %s\n", pw->pw_name);
    }
    else
    {
        printf("Error: Could not retrieve username.\n");
    }

    //gaze point stream
    lsl::stream_info gazePointOutletInfo(std::string("TobiiGazePoint"),
                          std::string("GAZE"),
                          2,
                          90,
                          lsl::cf_float32,
                          std::string(pw->pw_name));
    printf("!!! Create gaze point stream\n");
    gazePointOutlet = new lsl::stream_outlet(gazePointOutletInfo, 0);

    //gaze origin stream
    lsl::stream_info gazeOriginOutletInfo(std::string("TobiiGazeOrigin"),
                          std::string("GAZE"),
                          6,
                          90,
                          lsl::cf_float32,
                          std::string(pw->pw_name));

    printf("!!! Create gaze origin stream\n");
    gazeOriginOutlet = new lsl::stream_outlet(gazeOriginOutletInfo, 0);

    //eye position normalized stream
    lsl::stream_info eyePositionNormalizedOutletInfo(std::string("TobiiEyePositionNormalized"),
                          std::string("GAZE"),
                          6,
                          90,
                          lsl::cf_float32,
                          std::string(pw->pw_name));

    printf("!!! Create eye position normalized stream\n");
    eyePositionNormalizedOutlet = new lsl::stream_outlet(eyePositionNormalizedOutletInfo, 0);

    //eye position normalized stream
    lsl::stream_info userPresenceOutletInfo(std::string("TobiiUserPresence"),
                          std::string("GAZE"),
                          1,
                          lsl::IRREGULAR_RATE,
                          lsl::cf_string,
                          std::string(pw->pw_name));

    printf("!!! Create user presence stream\n");
    userPresenceOutlet = new lsl::stream_outlet(userPresenceOutletInfo, 0);
}
void deinitLSL()
{
    if(gazePointOutlet)
    {
        delete gazePointOutlet;
        gazePointOutlet = nullptr;
    }

    if(gazeOriginOutlet)
    {
        delete gazeOriginOutlet;
        gazeOriginOutlet = nullptr;
    }

    if(eyePositionNormalizedOutlet)
    {
        delete eyePositionNormalizedOutlet;
        eyePositionNormalizedOutlet = nullptr;
    }

    if(userPresenceOutlet)
    {
        delete userPresenceOutlet;
        userPresenceOutlet = nullptr;
    }
}

int main()
{
    tobii_error_t error = TOBII_ERROR_NO_ERROR;

    std::thread input_handler(inputThread);

    //init Tobii and LSL stuff
    initLSL();
    initTobii();

    printf("!!! run callbacks processing\n");
    while(!exit_flag)
    {
        error = tobii_wait_for_callbacks(1, &device);
        if((error != TOBII_ERROR_NO_ERROR)&&(error != TOBII_ERROR_TIMED_OUT))
        {
            printf("tobii_wait_for_callbacks() call error: %s\n", tobii_error_message(error));
        }

        error = tobii_device_process_callbacks(device);
        if(error != TOBII_ERROR_NO_ERROR)
        {
            printf("tobii_device_process_callbacks() call error: %s\n", tobii_error_message(error));
        }
    }

    input_handler.join(); // Wait for the input thread to finish

    //deinit Tobii and LSL stuff
    deinitTobii();
    deinitLSL();

    return 0;
}