#include "aio.h"
#include "log.h"

#include <math.h>
#include <stdio.h>
#include <string.h>

static constexpr uint16_t usb_timeout = 250;
static constexpr uint8_t buf_size = 32; // needs 32 bc of overflow issue on these aios?
static constexpr uint8_t max_curve_points = 7;

static struct aio_params_t params = {
	.fans = {
		.setpoint = 43.5f,
		.min_duty = 20,
		.max_duty = 80,
		.fan_stop = false,
		.ctrl = {
			.Ki = 0.1f,
			.Kd = 10.0f
		},
	}};

// posts cmd to the aio, puts n bytes of response inside resp buffer
#define POST_AND_GET_RESP(cmd, n) \
	uint8_t req = cmd;        \
	uint8_t resp[buf_size];   \
	aio_post(dev, &req, 1, resp, n)

// posts 2 bytes of cmd to aio, puts n bytes of response inside resp buffer
#define POST_AND_GET_RESP2(cmd1, cmd2, n) \
	uint8_t req[2] = {cmd1, cmd2};    \
	uint8_t resp[buf_size];           \
	aio_post(dev, req, 2, resp, n)

// set the duty of both fans at once
#define SET_FANS_DUTY(dev, duty)  \
	const uint8_t d = {duty}; \
	aio_set_fan_duty(dev, &d)

static inline float clamp(const float value, const float min, const float max) {
	return fmaxf(fminf(value, max), min);
}


static void aio_usb_claim(libusb_device_handle *dev) {
	int ret = libusb_claim_interface(dev, 0);
	if (ret != 0) {
		applog(LOG_ERR, "failed to claim AIO device: %s", libusb_error_name(ret));
		return;
	}
	ret = libusb_control_transfer(dev, (LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE), 0x02, 0x02, 0, nullptr, 0, usb_timeout);
	if (ret < 0)
		applog(LOG_ERR, "AIO clear-to-send failed: %s", libusb_error_name(ret));
}

static void aio_usb_release(libusb_device_handle *dev) {
	libusb_release_interface(dev, 0);
}

static void aio_post(libusb_device_handle *dev, uint8_t *req, const int req_len, uint8_t *resp, const int resp_len) {
	// applog(LOG_DBG, "writing to aio, %02x", req[0]);
	libusb_bulk_transfer(dev, 0x01, req, req_len, nullptr, usb_timeout);
	libusb_bulk_transfer(dev, 0x81, resp, resp_len, nullptr, usb_timeout);
}

static float aio_get_water_temp(libusb_device_handle *dev) {
	static float last_read = 36.6f; // sane init value so it doenst ramp the fan up
	POST_AND_GET_RESP(0xa9, 6);
	const float temp = (float)resp[3] + (float)resp[4] / 10.0f;

	if (temp > 10.0f && temp < 65.0f) {
		constexpr float max_change = 0.5f; // 0.5C change per refresh cycle
		const float delta = temp - last_read;
		last_read += fmaxf(fminf(delta, max_change), -max_change);
	}

	return last_read;
}

static uint16_t aio_get_pump_speed(libusb_device_handle *dev) {
	POST_AND_GET_RESP(0x31, 5);
	return (uint16_t)(resp[3] << 8 | resp[4]);
}

static enum aio_pump_mode_t aio_get_pump_mode(libusb_device_handle *dev) {
	POST_AND_GET_RESP(0x33, 4);
	return (enum aio_pump_mode_t)resp[3];
}

static void aio_get_fan_speed(libusb_device_handle *dev, uint16_t *speeds) {
	for (uint8_t i = 0; i < 2; i++) {
		POST_AND_GET_RESP2(0x41, i, 6);
		speeds[i] = (uint8_t)(resp[4] << 8 | resp[5]);
	}
}

static void aio_set_fan_duty(libusb_device_handle *dev, const uint8_t *duty) {
	static uint8_t last_duty = 255;
	if (duty[0] == last_duty)
		return;
	uint8_t req[3] = {0x42, 0, 0};
	uint8_t resp[buf_size];
	for (uint8_t i = 0; i < 2; i++) {
		req[1] = i;
		req[2] = MIN(MAX(duty[i], 0), 100);
		if (params.fans.fan_stop && req[2] <= params.fans.min_duty)
			req[2] = 0;
		aio_post(dev, req, sizeof(req), resp, buf_size);
		last_duty = req[2];
	}
}

static char *aio_get_firmware_version(libusb_device_handle *dev, char *buf) {
	POST_AND_GET_RESP(0xaa, buf_size);
	sprintf(buf, "%d.%d.%d.%d", resp[3], resp[4], resp[5], resp[6]);
	return buf;
}

static void aio_set_pump_mode(libusb_device_handle *dev, const enum aio_pump_mode_t mode) {
	static enum aio_pump_mode_t last_mode = UNKNOWN;
	if (mode == last_mode)
		return;
	applog(LOG_DBG, "setting pump to %u", mode);
	POST_AND_GET_RESP2(0x32, (uint8_t)mode, 6);
	last_mode = mode;
}

static void aio_set_color(libusb_device_handle *dev, const enum aio_color_mode_t mode, const uint8_t speed, const struct rgb_t *colors, const uint8_t color_count) {
	if (colors == nullptr) // NOLINT no its not
		return;

	constexpr uint8_t modes[] = {0x52, 0x55, 0x55, 0x58};
	uint8_t req[buf_size], resp[buf_size];

	req[0] = 0x56;
	req[1] = color_count;

	// sort out and send color table
	memcpy(&req[2], colors, color_count * 3);
	if (color_count < 2 || mode == AIO_COLOR_FIXED) {
		memcpy(&req[5], colors, 3); // copy the same color to the 2nd pos in the table
		req[1] = 2;
	}
	aio_post(dev, req, 2 + req[1] * 3, resp, buf_size);

	// send speed, skip if fixed color
	if (mode != AIO_COLOR_FIXED) {
		constexpr uint8_t speed_shift[] = {0x46, 0x28, 0x0F};
		constexpr uint8_t speed_pulse[] = {0x50, 0x37, 0x1E};
		req[0] = 0x53;
		req[1] = mode == AIO_COLOR_SHIFT ? speed_shift[speed] : speed_pulse[speed];
		aio_post(dev, req, 2, resp, buf_size);
	}

	// actually set the aio to desired mode
	req[0] = modes[mode];
	req[1] = 0x01;
	aio_post(dev, req, 2, resp, buf_size);
}

static void aio_set_fan_curve(libusb_device_handle *dev, const struct curve_point_t *curve, const uint8_t count) {
	if (curve == nullptr)
		return;

	if (count < 1 || count > max_curve_points) {
		applog(LOG_ERR, "aio: curve must have 1-%d points", max_curve_points);
		return;
	}

	struct curve_point_t padded[max_curve_points];
	for (int i = 0; i < max_curve_points; i++)
		padded[i] = (i < count) ? curve[i] : curve[count - 1];

	uint8_t req[16], resp[32];
	req[0] = 0x40;

	// setup request
	for (int i = 0; i < max_curve_points; i++)
		req[2 + i] = padded[i].temp;
	for (int i = 0; i < max_curve_points; i++)
		req[9 + i] = padded[i].duty;

	// set curve for both fans
	for (uint8_t i = 0; i < 2; i++) {
		req[1] = i;
		aio_post(dev, req, sizeof(req), resp, sizeof(resp));
	}
	applog(LOG_INF, "set up AIO fan curve");
}

void aio_detect(struct aio_t *aio) {
	libusb_init(nullptr);
	aio->handle = libusb_open_device_with_vid_pid(nullptr, AIO_USB_VID, AIO_USB_PID);
	if (aio->handle == nullptr) {
		applog(LOG_ERR, "Can't open AIO USB device %x:%x", AIO_USB_VID, AIO_USB_PID);
		return;
	}

	/* detaches other software that might take the device? */
	if (libusb_kernel_driver_active(aio->handle, 0))
		libusb_detach_kernel_driver(aio->handle, 0);

	aio_usb_claim(aio->handle);
	applog(LOG_INF, "found Corsair AIO device at %04x:%04x, fw ver: %s. Set point is %.1fC, fan stop %s", AIO_USB_VID, AIO_USB_PID, aio_get_firmware_version(aio->handle, (char[16]){0}), (double)params.fans.setpoint, params.fans.fan_stop ? "ENABLED" : "DISABLED");
	aio_set_pump_mode(aio->handle, QUIET); // needed for init ig?
	aio_set_color(aio->handle, AIO_COLOR_FIXED, 0, nullptr, 0); // so it doenst complain the func is unused
	aio_usb_release(aio->handle);
}

void aio_update(struct aio_t *aio) {
	if (aio->handle == nullptr)
		return;

	aio_usb_claim(aio->handle);

	aio->water_temp = aio_get_water_temp(aio->handle);
	aio->pump_speed = aio_get_pump_speed(aio->handle);
	aio->pump_mode = aio_get_pump_mode(aio->handle);
	aio_get_fan_speed(aio->handle, aio->fan_speed);

	/* fan logic */
	static float fan_duty = 0.0f;
	static float prev_temp = 36.0f; // 20 avoids huge jump of slope at boot
	const float error = aio->water_temp - params.fans.setpoint;
	const float slope = aio->water_temp - prev_temp;
	prev_temp = aio->water_temp;

	fan_duty += error * params.fans.ctrl.Ki + slope * params.fans.ctrl.Kd;
	fan_duty = clamp(fan_duty, params.fans.min_duty, params.fans.max_duty);

	applog(LOG_DBG, "Fan duty %.3f, error %.4f, slope %.4f", (double)fan_duty, (double)error * (double)params.fans.ctrl.Ki, (double)slope * (double)params.fans.ctrl.Kd);
	SET_FANS_DUTY(aio->handle, (uint8_t)fan_duty);

	/* pump logic */
	if (fan_duty > (float)params.fans.max_duty * 0.75f && aio->pump_counter < 300)
		aio->pump_counter++;
	if (fan_duty < (float)params.fans.min_duty * 1.75f && aio->pump_counter > 0)
		aio->pump_counter--;

	if (aio->pump_counter > 280)
		aio_set_pump_mode(aio->handle, EXTREME);
	if (aio->pump_counter > 60)
		aio_set_pump_mode(aio->handle, BALANCED);
	if (aio->pump_counter == 0)
		aio_set_pump_mode(aio->handle, QUIET);
	// applog(LOG_DBG, "aio->pump_counter is %d", aio->pump_counter);

	aio_usb_release(aio->handle);
}

void aio_close(const struct aio_t *aio) {
	const struct curve_point_t fan_curve[] = {
		{ .temp = 34, .duty = 20},
		{ .temp = 35, .duty = 35 },
		{ .temp = 39, .duty = 50 },
		{ .temp = 46, .duty = 75 },
		{ .temp = 55, .duty = 100}
	};

	aio_set_pump_mode(aio->handle, BALANCED);
	aio_set_fan_curve(aio->handle, fan_curve, sizeof(fan_curve) / sizeof(fan_curve[0]));
	libusb_attach_kernel_driver(aio->handle, 0); // reattach so liquidctl still works
	libusb_close(aio->handle);
	libusb_exit(nullptr);
	applog(LOG_INF, "AIO device closed");
}