#define F_CPU 20000000UL
#include <pololu/orangutan.h>
#include <stdio.h>
#include <stdlib.h>
#include "i2cmaster.h"
#define COMPASS 0x3C

int main()
{
    play_from_program_space(PSTR(">g32>>c32"));  // Play welcoming notes.

    lcd_goto_xy(0,0);
    print("i2c init");
    i2c_init();
    i2c_start_wait(COMPASS + I2C_WRITE);
    i2c_write(0x00);
    i2c_write(0x70);

    i2c_start_wait(COMPASS + I2C_WRITE);
    i2c_write(0x01);
    i2c_write(0xA0);

    i2c_start_wait(COMPASS + I2C_WRITE);
    i2c_write(0x02);
    i2c_write(0x00);
    lcd_goto_xy(0, 0);
    print("i2c init done");
    delay_ms(1000);

    int buffer[6];
    int x, y, z = 0;
    short i;

    char* msg = malloc(sizeof(char)*16);
    while(1)
    {
        i2c_start(COMPASS + I2C_READ);
        i2c_write(0x06);

        for (i = 0; i < 6; i++) {
            buffer[i] = i2c_readNak();
        }

        x = (buffer[0] << 8) | buffer[1];
        z = (buffer[2] << 8) | buffer[3];
        y = (buffer[4] << 8) | buffer[5];

        lcd_goto_xy(0, 1);
        sprintf(msg, "%d %d %d", x, y, z);
        print(msg);

        delay_ms(1000);
        lcd_goto_xy(0, 1);
        print("                ");
        delay_ms(200);
    }
}