#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <stdint.h>
#include <stdlib.h>
#include <linux/i2c-dev.h>
#include <linux/uinput.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#define NUNCHUK_ADDRESS     0x52
#define NUNCHUK_DATA_LENGTH 0x06
#define DEFAULT_ADAPTER     2
#define DEFAULT_UPDATE_FREQ 100

static const char* adapter = "/dev/i2c-1";
static int fd = -1, ret = -1;
static int update_freq  = DEFAULT_UPDATE_FREQ;
static int update_period = 0;
static int nunchuk_inited = 0;

struct i2c_smbus_ioctl_data ioctl_data;
uint8_t nunchuk_buffer[6];

void nunchuk_init();
void nunchuk_request_data();
void nunchuk_read_data();
void nunchuk_process_data();
static void catch_signal(int signal);


int main(int argc, char *argv[])
{
    signal(SIGTERM, catch_signal);
    signal(SIGKILL, catch_signal);
    signal(SIGINT, catch_signal);

    fd = open(adapter, O_RDWR);

    if(fd < 0) {
        printf("Error opening adapter %s\n", adapter);
        perror("open");
        exit(EXIT_FAILURE);
    }
    usleep(1000);

    ret = ioctl(fd, I2C_SLAVE, NUNCHUK_ADDRESS);
    if(ret < 0) {
        perror("ioctl I2C_SLAVE");
        close(fd);
        exit(EXIT_FAILURE);
    }

    update_period = (1/(float)update_freq) * 1000 * 1000;

    usleep(1000);
    nunchuk_init();
    usleep(1000);

    while(1) {
        if(nunchuk_inited) {
            nunchuk_request_data();
            usleep(100);
            nunchuk_read_data();
            nunchuk_process_data();

            usleep(update_period);
        } else {
            nunchuk_init();
            usleep(100 * 1000);
        }
    }


    close(fd);
    printf("\nn2pi exit\n");
    return 0;
}


void nunchuk_process_data()
{
    uint8_t joyX, joyY, Z, C;
    int aX, aY, aZ;

    joyX = nunchuk_buffer[0];
    joyY = nunchuk_buffer[1];
    Z = ~(nunchuk_buffer[5] & 0b1);
    C = ~(nunchuk_buffer[5] & 0b10);

    aX = (((nunchuk_buffer[5]>>2) & 0b11) | ((int)nunchuk_buffer[2])<<2);
    aY = (((nunchuk_buffer[5]>>4) & 0b11) | ((int)nunchuk_buffer[3])<<2);
    aZ = (((nunchuk_buffer[5]>>6) & 0b11) | ((int)nunchuk_buffer[4])<<2);

    printf("joyX: %d  joyY: %d  Z: %d  C: %d\n", joyX, joyY, Z, C);
    printf("aX: %d  aY: %d  aZ: %d\n", aX, aY, aZ);
}


void nunchuk_read_data()
{
    ret = i2c_smbus_read_block_data(fd, 0, nunchuk_buffer);
    if(ret < 0) {
        printf("error reading data\n");
    } else {
        printf("read %d byte(s)!!\n", ret);
    }
}

void nunchuk_request_data()
{
    if(i2c_smbus_write_byte_data(fd, 0x00, 0x00) < 0) {
        printf("error requesting data: smbus_write_byte error\n");
    }
}

void nunchuk_init()
{
    if(i2c_smbus_write_byte_data(fd, 0xF0, 0x55) < 0) {
        perror("error initializing nunchuk (step 1): smbus_write_byte error \n");
        nunchuk_inited = 0;
        return;
    }
    if(i2c_smbus_write_byte_data(fd, 0xFB, 0x00) < 0) {
        perror("error initializing nunchuk (step 2): smbus_write_byte error \n");
        nunchuk_inited = 0;
        return;
    }
    nunchuk_inited = 1;
}

static void catch_signal(int signal)
{
    printf("signal %i caught!\n", signal);
    if(fd > 0) close(fd);
    exit(-1);
}