#include <stdlib.h>
#include "stm32f4xx.h"
#include "gpio.h"
#include "i2c.h"
#include "dwt.h"

struct i2c {
    GPIO_TypeDef *scl_port;
    uint32_t scl;

    GPIO_TypeDef *sda_port;
    uint32_t sda;

    unsigned int ticks;
};

#define DELAY_TICKS (4)

extern GPIO_TypeDef *gpio_port(uint32_t pin);

#define SCL_H(i2c)  (((i2c)->scl_port)->BSRRL) = (((i2c)->scl) & PIN_MASK)
#define SCL_L(i2c)  (((i2c)->scl_port)->BSRRH)  = (((i2c)->scl) & PIN_MASK)
#define SDA_H(i2c)  (((i2c)->sda_port)->BSRRL) = (((i2c)->sda) & PIN_MASK)
#define SDA_L(i2c)  (((i2c)->sda_port)->BSRRH)  = (((i2c)->sda) & PIN_MASK)
#define	SCL_IN(i2c) ((((i2c)->scl_port)-IDR) & (((i2c)->sda) & PIN_MASK))
#define SDA_IN(i2c) ((((i2c)->sda_port)->IDR) & (((i2c)->sda) & PIN_MASK))

i2c_t i2c_new(uint32_t scl, uint32_t sda, int speed)
{
    i2c_t i2c = malloc(sizeof *i2c);
    if (i2c == NULL) {
        return NULL;
    }
    i2c->scl = scl;
    i2c->scl_port = gpio_port(scl);
    i2c->sda = sda;
    i2c->sda_port = gpio_port(sda);

    gpio_init(i2c->scl, GPIO_OUTPUT | GPIO_FLAG_PU | GPIO_FLAG_OD | GPIO_SPEED_LOW);
    gpio_init(i2c->sda, GPIO_OUTPUT | GPIO_FLAG_PU | GPIO_FLAG_OD | GPIO_SPEED_LOW);

    SCL_H(i2c);
    SDA_H(i2c);

    if (speed > 0) {
        i2c->ticks = 4000000 / speed / 2;
    } else {
        i2c->ticks = DELAY_TICKS;
    }

    return (i2c);
}

void i2c_free(i2c_t i2c)
{
    free(i2c);
}

int i2c_start(i2c_t i2c)
{
    int en, err = 0;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }

    SDA_H(i2c);
    SCL_H(i2c);

    dwt_wait_us(i2c->ticks);

    if (!SDA_IN(i2c)) { /* sda busy ? */
        err = -1;
        goto err;
    }

    SDA_L(i2c);

    dwt_wait_us(i2c->ticks);

    if (SDA_IN(i2c)) { /* sda hi, error. */
        err = -2;
        goto err;
    }

    SDA_L(i2c);

    dwt_wait_us(i2c->ticks);

err:
    if (!en) {
        dwt_close();
    }

    return err;
}

void i2c_stop(i2c_t i2c)
{
    int en;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }

    SCL_L(i2c);

    dwt_wait_us(i2c->ticks);

    SDA_L(i2c);

    dwt_wait_us(i2c->ticks);

    SCL_H(i2c);

    dwt_wait_us(i2c->ticks);

    SDA_H(i2c);

    dwt_wait_us(i2c->ticks);

    if (!en) {
        dwt_close();
    }
}

void i2c_ack(i2c_t i2c)
{
    int en;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }

    SCL_L(i2c);

    dwt_wait_us(i2c->ticks);

    SDA_L(i2c);

    dwt_wait_us(i2c->ticks);

    SCL_H(i2c);

    dwt_wait_us(i2c->ticks);

    SCL_L(i2c);

    dwt_wait_us(i2c->ticks);
    if (!en) {
        dwt_close();
    }
}

void i2c_nack(i2c_t i2c)
{
    int en;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }

    SCL_L(i2c);

    dwt_wait_us(i2c->ticks);

    SDA_H(i2c);

    dwt_wait_us(i2c->ticks);

    SCL_H(i2c);

    dwt_wait_us(i2c->ticks);

    SCL_L(i2c);

    dwt_wait_us(i2c->ticks);
    if (!en) {
        dwt_close();
    }
}

int i2c_wait_ack(i2c_t i2c)
{
    int en, err;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }

    SCL_L(i2c);

    dwt_wait_us(i2c->ticks);

    SDA_H(i2c);

    dwt_wait_us(i2c->ticks);

    SCL_H(i2c);

    dwt_wait_us(i2c->ticks);

    err = SDA_IN(i2c);

    SCL_L(i2c);
    if (!en) {
        dwt_close();
    }

    return err;
}

int i2c_send_byte(i2c_t i2c, uint8_t byte)
{
    int en, i = 8;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }
    while(i--) {
        SCL_L(i2c);

        dwt_wait_us(i2c->ticks);

        if(byte & 0x80)
            SDA_H(i2c);
        else
            SDA_L(i2c);

        byte <<= 1;

        dwt_wait_us(i2c->ticks);

        SCL_H(i2c);

        dwt_wait_us(i2c->ticks);
    }
    SCL_L(i2c);
    if (!en) {
        dwt_close();
    }
    return 0;
}

uint8_t i2c_recv_byte(i2c_t i2c)
{
    int en, i = 8;
    uint8_t rd = 0;

    if (!(en = dwt_is_enabled())) {
        dwt_init();
    }

    SDA_H(i2c);

    while(i--) {
        rd <<= 1;

        SCL_L(i2c);

        dwt_wait_us(i2c->ticks);

        SCL_H(i2c);

        dwt_wait_us(i2c->ticks);

        if (SDA_IN(i2c)) {
            rd |= 0x01;
        }
    }
    SCL_L(i2c);
    if (!en) {
        dwt_close();
    }
    return (rd);
}