void TWIinit()
{
    //set SCL to 400kHz
    TWSR = 0x00;
    TWBR = 0x0C;
    TWCR = (1<<TWEN);
}

uint8_t TWIstart()
{
    TWCR= (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) ;
    while ((TWCR & (1 << TWINT)) == 0);
    return (TWSR & 0xF8) != TW_START;
}

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

void TWIwrite(uint8_t data)
{
    TWDR=data;
    TWCR = (1 << TWINT) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));
}

uint8_t TWIreadACK()
{
    TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);
    while (!(TWCR & (1 << TWINT)));
    return TWDR;
}

uint8_t TWIreadNACK()
{
    TWCR = (1 << TWINT) | (1 << TWEN);
    while (!(TWCR & (1 << TWINT)));
    return TWDR;
}

uint8_t TWIGetStatus()
{
    uint8_t status;
    status = TWSR & 0xF8;
    return status;
}

void writeReg(uint8_t addr, uint8_t reg, uint8_t data)
{
    TWIstart();
    TWIwrite(addr);
    TWIwrite(reg);
    TWIwrite(data);
    TWIstop();
}

void readReg(uint8_t addr, uint8_t reg, uint8_t *data)
{
    TWIstart();
    TWIwrite(addr);
    TWIwrite(reg);
    TWIstart();
    TWIwrite(addr | 1);
    *data = TWIreadNACK();
    TWIstop();
}

void readRegstart(uint8_t addr, uint8_t reg)
{
    TWIstart();
    TWIwrite(addr);
    TWIwrite(reg);
    TWIstart();
    TWIwrite(addr | 1);
}