#include <avr/io.h>

#define ENABTW ((1<<TWINT)|(1<<TWEN)|(0<<TWIE))   // 0x80 4 1
#define START TWCR = (ENABTW|(1<<TWSTA))  // 0x20
#define STOP TWCR = (ENABTW|(1<<TWSTO))  // 0x10
#define SEND(x)  TWDR = x;  TWCR = ENABTW;
#define RECV(ack) TWCR = ENABTW | (ack? (1<<TWEA) : 0 );
unsigned char twista;
unsigned twitmo;
#define WAIT twitmo=0; while (!((twista = TWCR) & (1 << TWINT)) && ++twitmo);

/////===================================////////////////////
void TWIinit(void)
{
    DDRC &= ~0x30;              // pullup
    PORTC |= 0x30;              // pullup
    TWBR = 2;                   // 400 khz
    TWCR |= (1 << TWEN);
}

void TWIdocmd(unsigned char *msg)
{
    unsigned int mlen, rdwrf;

    while ((mlen = *msg++)) {
        rdwrf = *msg & 1;
        START;
        WAIT;
        do {
            SEND(*msg++);
            WAIT;
            // should check for ACK - twista == SAWA or SDWA
        } while (--mlen && !rdwrf);
        // read
        while (mlen--) {
            RECV(mlen);
            WAIT;
            *msg++ = TWDR;
        }
    }
    STOP;
}

#ifndef F_CPU
#define F_CPU 8000000
#endif
#include <util/delay.h>
#include <stdio.h>

static int serout(char c, FILE * stream)
{
    if (c == '\n') {
        while( !(UCSR0A & 0x20) );
        UDR0 = '\r';
    }
    while( !(UCSR0A & 0x20) );
    UDR0 = c;
    return 0;
}
static FILE fpuart = FDEV_SETUP_STREAM(serout, NULL, _FDEV_SETUP_WRITE);

void UART_Init(unsigned centibaud) {
    unsigned acc,count;
    // we don't do this often, and it should be more accurate than divides
#define BCLK (F_CPU/800)
    acc = 0;
    count = 0;
    while( acc < BCLK )
        acc += centibaud, count++;
    if( acc - BCLK > centibaud >> 1 )
        count--;
    UBRR0 = count - 1;

    UCSR0C = 0x06;              //8N1 (should be this from reset)
    UCSR0A = 0xE0 | 2;          // clear Interrupts, UART at 2x (xtal/8)
    UCSR0B = 0x18;              // oring in 0x80 would enable rx interrupt
    stdout = &fpuart;         //Required for printf init
}

unsigned char sendhmci[] = {
    5, 0x3c, 0, 0x18, 0, 0,
    0
};
unsigned char sendhmcpr[] = {
    2, 0x3c, 9,
    0
};
unsigned char sendhmcrd1[] = {
    2, 0x3d, 0,
    0
};
unsigned char sendhmcrd2[] = {
    7, 0x3d, 0, 0, 0, 0, 0, 0,
    0
};

int main(void)
{
    unsigned char cnt, sta;
    int xo, yo, zo;

#define BAUD 57600
    UART_Init(BAUD/10);

    for (;;) {
        TWIinit();
        printf("HMC5843\n");
        TWIdocmd(sendhmci);
        printf("Init2\n");
        TWIdocmd(sendhmci);
        printf("Ready\n");
        cnt = 0;
        while (cnt++ < 20) {
            TWIdocmd(sendhmcpr);
            TWIdocmd(sendhmcrd1);
            sta = sendhmcrd1[2];
            if (sta != 5)
                continue;
            cnt = 0;
            TWIdocmd(sendhmcrd2);
            xo = sendhmcrd2[3] | (sendhmcrd2[2] << 8);
            yo = sendhmcrd2[5] | (sendhmcrd2[4] << 8);
            zo = sendhmcrd2[7] | (sendhmcrd2[6] << 8);
            printf("%5d,%5d,%5d\n", xo, yo, zo);
            _delay_ms(12);
        }
    }
}