#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/i2c-dev.h>

#define SENSOR_ADDR 0x5C

int fd;

void init() {
	fd = open("/dev/i2c-0", O_RDWR);
	if(fd == -1) {
		perror("Error opening i2c fd");
		exit(1);
	}
	if(ioctl(fd, I2C_SLAVE, SENSOR_ADDR) < 0) {
		perror("Failed to acquire bus access and/or talk to slave.\n");
		exit(1);
	}
}

void end() {
	close(fd);
}

void i2c_send(uint8_t addr, uint8_t *data, size_t len) {
	write(fd, &addr, 1);
	write(fd, data, len);
}

uint8_t *i2c_read(size_t len, uint8_t *out) {
	if(out == NULL) {
		out = malloc(len);
	}

	read(fd, out, len);
	return out;
}

uint16_t crc16(uint8_t *data, size_t len) {
	uint16_t crc = 0xFFFF;
	while(len--) {
		crc ^= *data++;
		for(int i = 0; i < 8; i++) {
			if(crc & 1) {
				crc >>= 1;
				crc ^= 0xA001;
			} else {
				crc >>= 1;
			}
		}
	}
	return crc;
}

int read_data(float buf[2]) {
	static uint8_t cmd[3] = { 0x03, 0x00, 0x04 };
	uint8_t response[8];

	int valid = 0;
	
	for(int i = 0; i < 8; i++) {
		response[i] = 0xFF;
	}

	for(int i = 0; i < 5; i++) {
		i2c_send(SENSOR_ADDR, cmd, 3);
		nanosleep(&(struct timespec){.tv_sec = 0, .tv_nsec = 150000000L }, NULL);
		i2c_read(8, response);

		if(response[0] == 0x03 &&
		   response[1] == 0x04 &&
		   crc16(response, 6) == (response[6] | (response[7] << 8))) {
			valid = 1;
			break;
		}
	}

	if(valid) {
		int32_t rh = response[0] << 8 | response[1];
		int32_t tm = response[2] << 8 | response[3];

		if(tm & (1<<15)) {
			tm = -(tm ^ (1<<15));
		}

		buf[0] = rh * 0.1;
		buf[1] = tm * 0.1;
	}
	
	return valid;
}

int main(int argc, char **argv) {
	float data[2];

	init();
	
	for(int i = 0; i < 10; i++) {
		if(!read_data(data)) {
			continue;
		}

		printf("%4.1f %4.1f\n", data[0], data[1]);
	}

	end();
}