#include "xparameters.h"
#include "xiic.h"
#include "xil_printf.h"
#include "sleep.h"
#include <stdint.h>

#if defined(XPAR_AXI_IIC_0_BASEADDR)
#define IIC_BASEADDR XPAR_AXI_IIC_0_BASEADDR
#elif defined(XPAR_XIIC_0_BASEADDR)
#define IIC_BASEADDR XPAR_XIIC_0_BASEADDR
#else
#error "Do not find the  Base Address of AXI IIC. Review xparameters.h"
#endif

#define STTS22H_ADDR        0x3F
#define STTS22H_WHOAMI_REG  0x01
#define STTS22H_WHOAMI_VAL  0xA0
#define STTS22H_CTRL_REG    0x04
#define STTS22H_TEMP_L_REG  0x06

static int i2c_write_reg(uint8_t addr, uint8_t reg, uint8_t value)
{
    uint8_t buf[2] = {reg, value};

    int sent = XIic_DynSend(IIC_BASEADDR, addr, buf, 2, XIIC_STOP);

    return sent == 2 ? 0 : -1;
}

static int i2c_read_regs(uint8_t addr, uint8_t reg, uint8_t *buf, int len)
{
    int sent = XIic_DynSend(IIC_BASEADDR, addr, &reg, 1, XIIC_REPEATED_START);
    if (sent != 1) {
        return -1;
    }

    int received = XIic_DynRecv(IIC_BASEADDR, addr, buf, len);
    if (received != len) {
        return -1;
    }

    return 0;
}

int main(void)
{
    uint8_t whoami = 0;
    uint8_t temp_raw[2];
    int16_t raw;
    int temp_int;
    int temp_dec;

    xil_printf("\r\n=== ZUBoard 1CG - STTS22H I2C ===\r\n");

    XIic_DynInit(IIC_BASEADDR);

    if (i2c_read_regs(STTS22H_ADDR, STTS22H_WHOAMI_REG, &whoami, 1) != 0) {
        xil_printf("ERROR:  I2C not responding on 0x%02X\r\n", STTS22H_ADDR);
        while (1);
    }

    xil_printf("WHOAMI = 0x%02X\r\n", whoami);

    if (whoami != STTS22H_WHOAMI_VAL) {
        xil_printf("ERROR: WHOAMI. Expecting 0xA0.\r\n");
        while (1);
    }

    xil_printf("STTS22H detected correctly.\r\n");

    /*
     * CTRL = 0x0C: Configuration to readout continously the temperature.
     */
    if (i2c_write_reg(STTS22H_ADDR, STTS22H_CTRL_REG, 0x0C) != 0) {
        xil_printf("ERROR configurando CTRL.\r\n");
        while (1);
    }

    while (1) {
        if (i2c_read_regs(STTS22H_ADDR, STTS22H_TEMP_L_REG, temp_raw, 2) == 0) {
            raw = (int16_t)((temp_raw[1] << 8) | temp_raw[0]);

            temp_int = raw / 100;
            temp_dec = raw % 100;
            if (temp_dec < 0) {
                temp_dec = -temp_dec;
            }

            xil_printf("Temperature: %d.%02d C\r\n", temp_int, temp_dec);
        } else {
            xil_printf("ERROR reading temperature.\r\n");
        }

        sleep(1);
    }

    return 0;
}