#include <avr/io.h>
#include <util/delay.h>
#include <avr/wdt.h>
#include <avr/interrupt.h>
#include <util/twi.h>
#include <stdio.h>
#include <stdint.h>
#include "serial.h"

uint16_t loopCounter = 0;
uint8_t nunchuck_buffer[6];

void twi_init()
{
    TWBR = 72;
    TWSR = 0;
}

int8_t twi_start_condition()
{
    TWCR = (1<<TWINT)|(1<<TWSTA)| (1<<TWEN) | (0<<TWIE);

    loopCounter = 0;
    while (!(TWCR & (1<<TWINT)) && loopCounter < 0xFFFF) {loopCounter++; _delay_us(1);}
    if ((TWSR & 0xF8) != TW_START)
        return 0;
    else
        return 1;
}

int8_t twi_send_address_write(const int8_t address)
{
    TWDR = (address << 1);
    TWCR = (1<<TWINT) | (1<<TWEN);
    loopCounter = 0;
    while (!(TWCR & (1<<TWINT)) && loopCounter < 0xFFFF) {loopCounter++; _delay_us(1);}
    if ((TWSR & 0xF8) != TW_MT_SLA_ACK)
        return 0;
    else
        return 1;
}

int8_t twi_send_address_read(const int8_t address)
{
    TWDR = (address << 1) | 1;
    TWCR = (1<<TWINT) | (1<<TWEN);
    loopCounter = 0;
    while (!(TWCR & (1<<TWINT)) && loopCounter < 0xFFFF) {loopCounter++; _delay_us(1);}
    if ((TWSR & 0xF8) != TW_MR_SLA_ACK)
        return 0;
    else
        return 1;
}

int8_t twi_send_byte(const int8_t byte)
{
    TWDR = byte;
    TWCR = (1<<TWINT) | (1<<TWEN);
    loopCounter = 0;
    while (!(TWCR & (1<<TWINT)) && loopCounter < 0xFFFF) {loopCounter++; _delay_us(1);}
    if ((TWSR & 0xF8) != TW_MT_DATA_ACK)
        return 0;
    else
        return 1;
}

void twi_stop_transmission()
{
    TWCR = (1<<TWINT)|(1<<TWEN)| (1<<TWSTO);
}

int8_t nunchuck_init()
{
    if(!twi_start_condition()) {
        uart_puts("nunchuck_init(): Error transmitting start condition\n");
        return 1;
    }

    if(!twi_send_address_write(0x52)) {
        uart_puts("nunchuck_init(): Error sending address\n");
        return 1;
    }

    if(!twi_send_byte(0x40)) {
        uart_puts("nunchuck_init(): Error sending byte 0x40\n");
        return 1;
    }

    if(!twi_send_byte(0x00)) {
        uart_puts("nunchuck_init(): Error sending byte 0x00\n");
        return 1;
    }

    twi_stop_transmission();
    return 0;
}

int8_t nunchuck_request_data()
{
    if(!twi_start_condition()) {
        uart_puts("nunchuck_request_data(): Error transmitting start condition\n");
        return 1;
    }

    if(!twi_send_address_write(0x52)) {
        uart_puts("nunchuck_request_data(): Error sending address\n");
        return 1;
    }

    if(!twi_send_byte(0x00)) {
        uart_puts("nunchuck_request_data(): Error sending byte 0x00\n");
        return 1;
    }

    twi_stop_transmission();

    _delay_ms(1);

    if(!twi_start_condition()) {
        uart_puts("Error transmitting start condition 3\n");
        return 1;
    }

    if(!twi_send_address_read(0x52)) {
        uart_puts("Error sending address 3\n");
        return 1;
    }

    for(int8_t i = 0; i < 6; i++) {
        TWCR = (1<<TWINT) | (1<<TWEN) | _BV(TWEA);

        loopCounter = 0;
        while (!(TWCR & (1<<TWINT)) && loopCounter < 0xFFFF) {loopCounter++; _delay_us(1);}
        if ((TWSR & 0xF8) != TW_MR_DATA_ACK) {
            uart_puts("nunchuck_request_data(): nack");
            return 1;
        }
        nunchuck_buffer[i] = (TWDR ^ 0x17) + 0x17; // that's true cryptography
    }
    TWCR = (1<<TWINT) | (1<<TWEN); // send NAK
    twi_stop_transmission();

    return 0;
}

int8_t nunchuck_reset_and_read()
{
    TWCR = 0;
    _delay_ms(5);
    twi_init();

    nunchuck_init();

    _delay_ms(20);

    nunchuck_request_data();
    _delay_ms(200);
    return 0;
}

void nunchuck_dump_raw_data()
{
    for (int8_t i = 0; i < 6; i++) {
        uart_putint(nunchuck_buffer[i]);
        uart_putc('|');
    }
    uart_putc('\n');
}

inline int8_t nunchuck_button_z()
{
    return (nunchuck_buffer[5] & 1) ^ 1;
}

inline int8_t nunchuck_button_c()
{
    return ((nunchuck_buffer[5] >> 1) & 1) ^ 1;
}

inline uint8_t nunchuck_joystick_x()
{
    return nunchuck_buffer[0];
}

inline uint8_t nunchuck_joystick_y()
{
    return nunchuck_buffer[1];
}

inline uint16_t nunchuck_accel_x()
{
    uint16_t ret = nunchuck_buffer[2];
    ret <<= 2;
    ret = ret + ((nunchuck_buffer[5] >> 2) & 0x03);
    return ret;
}

inline uint16_t nunchuck_accel_y()
{
    uint16_t ret = nunchuck_buffer[3];
    ret <<= 2;
    ret = ret + ((nunchuck_buffer[5] >> 4) & 0x03);
    return ret;
}

inline uint16_t nunchuck_accel_z()
{
    uint16_t ret = nunchuck_buffer[4];
    ret <<= 2;
    ret = ret + ((nunchuck_buffer[5] >> 6) & 0x03);
    return ret;
}

void dumme_funktion()
{
    uart_puts("WTF? WTF? WTF? WTF? WTF?WTF? WTF? WTF? WTF? WTF?WTF? WTF? WTF? WTF? WTF?WTF? WTF? WTF? WTF? WTF?WTF? WTF? WTF? WTF? WTF?");
}

void nunchuck_dump()
{
    uart_puts("\nButtons: ");
    if (nunchuck_button_z())
        uart_putc('Z');
    else
        uart_putc('_');
    if (nunchuck_button_c())
        uart_putc('C');
    else
        uart_putc('_');

    uart_puts(" Joystick: x-axis: ");
    for (int8_t i = 0; i <= 0xF; i++) {
        uart_putc(nunchuck_joystick_x() > (i<<4) ? 'X' : '-');
    }
    uart_puts(" y-axis: ");
    for (int8_t i = 0; i <= 0xF; i++) {
        uart_putc(nunchuck_joystick_y() > (i<<4) ? 'X' : '-');
    }
    uart_puts("\nx-axis: ");
    uint16_t temp = nunchuck_accel_x() >> 2;
    for (int8_t i = 0; i <= 0xF; i++) {
        uart_putc(temp > (i<<4) ? 'X' : '-');
    }
    uart_puts(" y-axis: ");
    temp = nunchuck_accel_y() >> 2;
    for (int8_t i = 0; i <= 0xF; i++) {
        uart_putc(temp > (i<<4) ? 'X' : '-');
    }
    uart_puts(" z-axis: ");
    temp = nunchuck_accel_z() >> 2;
    for (int8_t i = 0; i <= 0xF; i++) {
        uart_putc(temp > (i<<4) ? 'X' : '-');
    }
    uart_putc('\n');

}

void bench()
{
    _delay_ms(1000);
    while(1) {
        uart_puts("Starting\n");
        for (uint16_t i = 0; i < 100; i++) {
            nunchuck_reset_and_read();
        }
        uart_puts("DONE!\n\n");
    }
}

int main()
{
    sei();
    wdt_disable();
    uart_init();
    uart_putc('x');
//  bench();
    while(1) {
//      uart_putc('y');
        nunchuck_reset_and_read();
//      uart_putc('z');
//      nunchuck_dump_raw_data();
        nunchuck_dump();
    }

    return 0;
}