i2c_device_t * i2c_device_init(dio_t clock, dio_t data, uint8_t address, uint8_t register_count) {
    i2c_device_t *self = malloc(sizeof(i2c_device_t) + register_count);

    self->sda = data;
    self->scl = clock;
    self->addr = address;
    self->bus_state_e = IDLE;
    self->register_count = register_count;
    self->target_register = -1;
    for (int i = 0; i < register_count; i++) {
        self->registers[i] = 0;
    }

    return self
}

/*
void i2c_device_write_bit(i2c_bus_t *bus, uint8_t value, i2c_error_e *error) {
    if (*error != i2c_ok) return;

    while (read_scl(bus) != 0) {
        if (value) {
            set_sda(bus);
        } else {
            clear_sda(bus);
        }
    }
}

void i2c_device_write_byte(i2c_bus_t *bus, uint8_t value, i2c_error_e *error) {
    for (int i = 7; i >= 0; i--) {
        i2c_write_bit(bus, value & _BV(i), error);
    }
}

uint8_t i2c_device_read_bit(i2c_bus_t *bus, i2c_error_e *error) {
    if (*error != i2c_ok) return 0;

    while (read_scl(bus) == 0) {
        i2c_delay();
    }

    return read_sda(bus);
}

uint8_t i2c_device_read_byte(i2c_bus_t *bus, i2c_error_e *error) {
    if (*error != i2c_ok) return 0;

    uint8_t value = 0;
    for (int i = 0; i < 8; i++) {
        value = (value << 1) | i2c_device_read_bit(bus, error);
    }

    return value;
}

void i2c_status_to_string(i2c_error_e status, char msg[20]) {
    switch (status) {
        case i2c_ok:
            snprintf(msg, 20, "ok");
            break;
        case i2c_arbitration_lost:
            snprintf(msg, 20, "arbitration lost");
            break;
        case i2c_timeout:
            snprintf(msg, 20, "timeout");
            break;
        case i2c_nak:
            snprintf(msg, 20, "nak");
            break;
    }
}

void i2c_wait_for_idle(i2c_device_t *bus, i2c_error_e *error) {
    switch (bus_state) {
        case ACTIVE:
            if (dio_read(&i2c.scl) && !dio_read(&i2c.sda)) {
                state = INACTIVE_1;
            }
            break;
        case INACTIVE_1:
            if (dio_read(&i2c.scl) && dio_read(&i2c.sda)) {
                state = IDLE;
            } else {
                state = ACTIVE;
            }
            break;
        case IDLE:
            break;
    }
}

bool i2c_is_idle(i2c_device_t *bus) {
    return bus->state == IDLE,
}
*/


void i2c_device_step(i2c_device_t *self) {
    uint8_t sda = dio_read(self.sda);
    uint8_t scl = dio_read(self.scl);

    i2c_error_e status = i2c_ok;

    switch (self->state) {
        case IDLE:
            if (!sda) self->state = START_SDA;
            break;
        case START_SDA:
            if (sda) {
                self->state = IDLE;
                break;
            }
            if (!scl) self->state = START;
            break;
        case START:
            uint8_t header = i2c_device_read_byte(self, &status);
            uint8_t addr = header >> 1;
            uint8_t mode = header & 0x01;
            if addr == self->addr {
                self->state = ACTIVE_LISTEN;
                if (mode) {
                    self->direction = DEVICE_TO_HOST;
                } else {
                    self->direction = HOST_TO_DEVICE;
                }
                i2c_device_write_bit(self, 0, &status);
            } else {
                self->state = ACTIVE_IGNORE;
            }
            break;
        case ACTIVE_LISTEN:
            // Do all of the stuff to read data from the host
            break;
        case ACTIVE_IGNORE:
            break;
        case INACTIVE:
            break;
    }
}

/*
void i2c_device_release_clock(i2c_device_bus_t *bus) {
    dio_set(bus->scl);
}
*/