#include <windows.h>
#include <stdio.h>
#include "ioport.h"

/*
 * This is a hacked up piece of bit-banging code to query a Wii Motion Plus
 * over I2C on the parallel port. It's not polished code by a long way.
 * You have been warned.
 */

/* This is the first parallel port on most PCs. */
#define PAR 0x378

#define SCL_MASK 0x40
#define SDA_MASK 0x80
#define SDA_IN_MASK 0x40

#if 0
#define DELAY Sleep(1)
#else
int _delay;
#define DELAY for (_delay = 0; _delay < DELAY_COUNT; _delay++) {}
#endif

/*
 * Carefully Calibrated[tm] delay loop.
 * This works on my PC, it may not work on yours.
 * You have been warned.
 */
#define DELAY_COUNT 10000

#define NCAL 100

int roll0, pitch0, yaw0;

int paroutvalue = SCL_MASK | SDA_MASK;

void initparallel(void)
{
    paroutvalue = SCL_MASK | SDA_MASK;
    outportb(PAR+2, 0x20);
    outportb(PAR+0, paroutvalue);
    DELAY;
}

void setscl(int value)
{
    /* We have an inverter in the way */
    value = !value;

    paroutvalue &= ~SCL_MASK;
    paroutvalue |= (value ? SCL_MASK : 0);
    outportb(PAR+0, paroutvalue);
    DELAY;
}

void setsda(int value)
{
    /* We have an inverter in the way */
    value = !value;

    paroutvalue &= ~SDA_MASK;
    paroutvalue |= (value ? SDA_MASK : 0);
    outportb(PAR+0, paroutvalue);
    DELAY;
}

int getsda(void)
{
    int value;

    value = inportb(PAR+1) & SDA_IN_MASK;

    /* We have an inverter in the way */
    value = !value;

    return (value != 0);
}

void send_start(void)
{
	setsda(1);
	setscl(1);
	setsda(0);
}

void send_stop(void)
{
	setsda(0);
	setscl(1);
	setsda(1);
}

int send_byte(int value)
{
    int i;
    int ack;

    for (i = 0; i < 8; i++) {
	setscl(0);
	setsda(value & 0x80);
	setscl(1);
	value = value << 1;
    }
    setscl(0);
    setsda(1);
    setscl(1);
    ack = !getsda();
    setscl(0);
    if (!ack) {
	send_stop();
	printf("ACK not received\n");
	return FALSE;
    }
    return TRUE;
}

int get_byte(int last)
{
    int i;
    int value = 0;

    setsda(1);

    for (i = 0; i < 8; i++) {
	setscl(0);
	setscl(1);
	value = value << 1;
	value |= (getsda()?1:0);
    }
    setscl(0);
    if (!last)
        setsda(0);
    setscl(1);
    setscl(0);
    setsda(1);

    return value;
}

void setupi2c(void)
{
    send_start();
    send_byte((0x53 << 1) + 0); /* Address 0x53, write */
    send_byte(0xfe);
    send_byte(0x04);
    send_stop();
}

void get_readings(int *b)
{
    send_start();
    send_byte((0x52 << 1) + 0); /* Address 0x52, write */
    send_byte(0);
    send_stop();

    send_start();
    send_byte((0x52 << 1) + 1); /* Address 0x52, read */
    b[0] = get_byte(0);
    b[1] = get_byte(0);
    b[2] = get_byte(0);
    b[3] = get_byte(0);
    b[4] = get_byte(0);
    b[5] = get_byte(1);
    send_stop();
}

void query_device(int *roll, int *pitch, int *yaw)
{
    int b[6];

    get_readings(b);

    *yaw   = ((b[3]>>2)<<8) + b[0] - yaw0;
    *pitch = ((b[4]>>2)<<8) + b[1] - pitch0;
    *roll  = ((b[5]>>2)<<8) + b[2] - roll0;
}

void calibrate(void)
{
    int b[6];
    int i;

    yaw0 = 0;
    pitch0 = 0;
    roll0 = 0;

    printf("Calibrating... ");

    for (i = 0; i < NCAL; i++) {
	printf("%2d", i);
	get_readings(b);
        yaw0   += ((b[3]>>2)<<8) + b[0];
        pitch0 += ((b[4]>>2)<<8) + b[1];
        roll0  += ((b[5]>>2)<<8) + b[2];
	printf("\b\b");
    }

    yaw0   = yaw0   / NCAL;
    pitch0 = pitch0 / NCAL;
    roll0  = roll0  / NCAL;

    printf("done\n");
    printf("Calibrated values: %d, %d, %d\n", roll0, pitch0, yaw0);
}