#include <pico/stdlib.h>
#include <pico/binary_info.h>
#include <hardware/i2c.h>
#include <hardware/resets.h>
#include <hardware/clocks.h>
#include <pico/timeout_helper.h>

#include "protocfg.h"
#include "pinout.h"
#include "i2ctinyusb.h"

//static int delay = 10, delay2 = 5;
//
//// I2C bitbang reimpl because ugh, synopsys
//// (mostly inspired by original I2CTinyUSB AVR firmware)
//__attribute__((__always_inline__)) inline static void i2cio_set_sda(bool hi) {
//	if (hi) {
//		sio_hw->gpio_oe_clr = (1<<PINOUT_I2C_SDA); // SDA is input
//		// => pullup configured, so it'll go high
//	} else {
//		sio_hw->gpio_oe_set = (1<<PINOUT_I2C_SDA); // SDA is output
//		sio_hw->gpio_clr = (1<<PINOUT_I2C_SDA); // and drive it low
//	}
//}
//__attribute__((__always_inline__)) inline static bool i2cio_get_sda(void) {
//	return (sio_hw->gpio_in & (1<<PINOUT_I2C_SDA)) != 0;
//}
//__attribute__((__always_inline__)) inline static void i2cio_set_scl(bool hi) {
//	busy_wait_us_32(delay2);
//	sio_hw->gpio_oe_set = (1<<PINOUT_I2C_SCL); // SCL is output
//	if (hi)
//		sio_hw->gpio_set = (1<<PINOUT_I2C_SCL); // SCL is high
//	else
//		sio_hw->gpio_clr = (1<<PINOUT_I2C_SCL); // SCL is low
//	busy_wait_us_32(delay2);
//}
//
//__attribute__((__always_inline__)) inline static void i2cio_scl_toggle(void) {
//	i2cio_set_scl(true );
//	i2cio_set_scl(false);
//}
//
//static void __no_inline_not_in_flash_func(i2cio_start)(void) { // start condition
//	i2cio_set_sda(false);
//	i2cio_set_scl(false);
//}
//static void __no_inline_not_in_flash_func(i2cio_repstart)(void) { // repstart condition
//	i2cio_set_sda(true);
//	i2cio_set_scl(true);
//
//	i2cio_set_sda(false);
//	i2cio_set_scl(false);
//}
//static void __no_inline_not_in_flash_func(i2cio_stop)(void) { // stop condition
//	i2cio_set_sda(false);
//	i2cio_set_scl(true );
//	i2cio_set_sda(true );
//}
//
//static bool __no_inline_not_in_flash_func(i2cio_write7)(uint8_t v) { // return value: acked? // needed for 10bitaddr xfers
//	for (int i = 6; i >= 0; --i) {
//		i2cio_set_sda((v & (1<<i)) != 0);
//		i2cio_scl_toggle();
//	}
//
//	i2cio_set_sda(true);
//	i2cio_set_scl(true);
//
//	bool ack = i2cio_get_sda();
//	i2cio_set_scl(false);
//
//	return ack;
//}
//static bool __no_inline_not_in_flash_func(i2cio_write8)(uint8_t v) { // return value: acked?
//	for (int i = 7; i >= 0; --i) {
//		i2cio_set_sda((v & (1<<i)) != 0);
//		i2cio_scl_toggle();
//	}
//
//	i2cio_set_sda(true);
//	i2cio_set_scl(true);
//
//	bool ack = i2cio_get_sda();
//	i2cio_set_scl(false);
//
//	return ack;
//}
//static uint8_t __no_inline_not_in_flash_func(i2cio_read8)(bool last) {
//	i2cio_set_sda(true );
//	i2cio_set_scl(false);
//
//	uint8_t rv = 0;
//	for (int i = 7; i >= 0; --i) {
//		i2cio_set_scl(true);
//		bool c = i2cio_get_sda();
//		rv <<= 1;
//		if (c) rv |= 1;
//		i2cio_set_scl(false);
//	}
//
//	if (last) i2cio_set_sda(true);
//	else i2cio_set_sda(false);
//
//	i2cio_scl_toggle();
//	i2cio_set_sda(true);
//}
//
//// replicating/rewriting some SDK functions because they don't do what I want
//// so I'm making better ones
//
//static int __no_inline_not_in_flash_func(i2cex_probe_address)(uint16_t addr, bool a10bit) {
//	// I2C pins to SIO
//	gpio_set_function(PINOUT_I2C_SCL, GPIO_FUNC_SIO);
//	gpio_set_function(PINOUT_I2C_SDA, GPIO_FUNC_SIO);
//
//	int rv;
//	i2cio_start();
//
//	if (a10bit) {
//		//         A10 magic   higher 2 addr bits         r/#w bit
//		uint8_t addr1 = 0x70 | (((addr >> 8) & 3) << 1) | 0,
//		        addr2 = addr & 0xff;
//
//		if (i2cio_write7(addr1)) {
//			if (i2cio_write8(addr2)) rv = 0;
//			else rv = PICO_ERROR_GENERIC;
//		} else rv = PICO_ERROR_GENERIC;
//	} else {
//		if (i2cio_write8((addr << 1) & 0xff)) rv = 0; // acked: ok
//		else rv = PICO_ERROR_GENERIC; // nak :/
//	}
//	i2cio_stop();
//
//	// I2C back to I2C
//	gpio_set_function(PINOUT_I2C_SCL, GPIO_FUNC_I2C);
//	gpio_set_function(PINOUT_I2C_SDA, GPIO_FUNC_I2C);
//
//	return rv;
//}
//
//static void i2cex_abort_xfer(i2c_inst_t* i2c) {
//	// now do the abort
//	i2c->hw->enable = 1 /*| (1<<2)*/ | (1<<1);
//	// wait for M_TX_ABRT irq
//	do {
//		/*if (timeout_check) {
//			timeout = timeout_check(ts);
//			abort |= timeout;
//		}*/
//		tight_loop_contents();
//	} while (/*!timeout &&*/ !(i2c->hw->raw_intr_stat & I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS));
//	// reset irq
//	//if (!timeout)
//		(void)i2c->hw->clr_tx_abrt;
//}
//
//static int i2cex_write_blocking_until(i2c_inst_t* i2c, uint16_t addr, bool a10bit,
//		const uint8_t* src, size_t len, bool nostop, absolute_time_t until) {
//	timeout_state_t ts_;
//	struct timeout_state* ts = &ts_;
//	check_timeout_fn timeout_check = init_single_timeout_until(&ts_, until);
//
//	if ((int)len < 0) return PICO_ERROR_GENERIC;
//	if (a10bit) { // addr too high
//		if (addr & ~(uint16_t)((1<<10)-1)) return PICO_ERROR_GENERIC;
//	} else if (addr & 0x80)
//		return PICO_ERROR_GENERIC;
//
//	if (len == 0) return i2cex_probe_address(addr, a10bit);
//
//	bool abort = false, timeout = false;
//	uint32_t abort_reason = 0;
//	int byte_ctr;
//
//	i2c->hw->enable = 0;
//	// enable 10bit mode if requested
//	hw_write_masked(&i2c->hw->con, I2C_IC_CON_IC_10BITADDR_MASTER_BITS, (a10bit
//			? I2C_IC_CON_IC_10BITADDR_MASTER_VALUE_ADDR_10BITS
//			: I2C_IC_CON_IC_10BITADDR_MASTER_VALUE_ADDR_7BITS ) << I2C_IC_CON_IC_10BITADDR_MASTER_LSB);
//	i2c->hw->tar = addr;
//	i2c->hw->enable = 1;
//
//	for (byte_ctr = 0; byte_ctr < (int)len; ++byte_ctr) {
//		bool first = byte_ctr == 0,
//		     last  = byte_ctr == (int)len - 1;
//
//		i2c->hw->data_cmd =
//			bool_to_bit(first && i2c->restart_on_next) << I2C_IC_DATA_CMD_RESTART_LSB |
//			bool_to_bit(last  && !nostop) << I2C_IC_DATA_CMD_STOP_LSB |
//			*src++;
//
//		do {
//			if (timeout_check) {
//				timeout = timeout_check(ts);
//				abort |= timeout;
//			}
//			tight_loop_contents();
//		} while (!timeout && !(i2c->hw->raw_intr_stat & I2C_IC_RAW_INTR_STAT_TX_EMPTY_BITS));
//
//		if (!timeout) {
//			abort_reason = i2c->hw->tx_abrt_source;
//			if (abort_reason) {
//				(void)i2c->hw->clr_tx_abrt;
//				abort = true;
//			}
//
//			if (abort || (last && !nostop)) {
//				do {
//					if (timeout_check) {
//						timeout = timeout_check(ts);
//						abort |= timeout;
//					}
//					tight_loop_contents();
//				} while (!timeout && !(i2c->hw->raw_intr_stat & I2C_IC_RAW_INTR_STAT_STOP_DET_BITS));
//
//				if (!timeout) (void)i2c->hw->clr_stop_det;
//				else
//					// if we had a timeout, send an abort request to the hardware,
//					// so that the bus gets released
//					i2cex_abort_xfer(i2c);
//			}
//		} else i2cex_abort_xfer(i2c);
//
//		if (abort) break;
//	}
//
//	int rval;
//
//	if (abort) {
//		const int addr_noack = I2C_IC_TX_ABRT_SOURCE_ABRT_7B_ADDR_NOACK_BITS
//		                     | I2C_IC_TX_ABRT_SOURCE_ABRT_10ADDR1_NOACK_BITS
//		                     | I2C_IC_TX_ABRT_SOURCE_ABRT_10ADDR2_NOACK_BITS;
//
//		if (timeout) rval = PICO_ERROR_TIMEOUT;
//		else if (!abort_reason || (abort_reason & addr_noack))
//			rval = PICO_ERROR_GENERIC;
//		else if (abort_reason & I2C_IC_TX_ABRT_SOURCE_ABRT_TXDATA_NOACK_BITS)
//			rval = byte_ctr;
//		else rval = PICO_ERROR_GENERIC;
//	} else rval = byte_ctr;
//
//	i2c->restart_on_next = nostop;
//	return rval;
//}
//static int i2cex_read_blocking_until(i2c_inst_t* i2c, uint16_t addr, bool a10bit,
//		uint8_t* dst, size_t len, bool nostop, absolute_time_t until) {
//	timeout_state_t ts_;
//	struct timeout_state* ts = &ts_;
//	check_timeout_fn timeout_check = init_single_timeout_until(&ts_, until);
//
//	if ((int)len < 0) return PICO_ERROR_GENERIC;
//	if (a10bit) { // addr too high
//		if (addr & ~(uint16_t)((1<<10)-1)) return PICO_ERROR_GENERIC;
//	} else if (addr & 0x80)
//		return PICO_ERROR_GENERIC;
//
//	i2c->hw->enable = 0;
//	// enable 10bit mode if requested
//	hw_write_masked(&i2c->hw->con, I2C_IC_CON_IC_10BITADDR_MASTER_BITS, (a10bit
//			? I2C_IC_CON_IC_10BITADDR_MASTER_VALUE_ADDR_10BITS
//			: I2C_IC_CON_IC_10BITADDR_MASTER_VALUE_ADDR_7BITS ) << I2C_IC_CON_IC_10BITADDR_MASTER_LSB);
//	i2c->hw->tar = addr;
//	i2c->hw->enable = 1;
//
//	if (len == 0) return i2cex_probe_address(addr, a10bit);
//
//	bool abort = false, timeout = false;
//	uint32_t abort_reason = 0;
//	int byte_ctr;
//
//	for (byte_ctr = 0; byte_ctr < (int)len; ++byte_ctr) {
//		bool first = byte_ctr == 0;
//		bool last  = byte_ctr == (int)len - 1;
//
//		while (!i2c_get_write_available(i2c) && !abort) {
//			tight_loop_contents();
//			// ?
//			if (timeout_check) {
//				timeout = timeout_check(ts);
//				abort |= timeout;
//			}
//		}
//
//		if (timeout) {
//			// if we had a timeout, send an abort request to the hardware,
//			// so that the bus gets released
//			i2cex_abort_xfer(i2c);
//		}
//		if (abort) break;
//
//		i2c->hw->data_cmd =
//			bool_to_bit(first && i2c->restart_on_next) << I2C_IC_DATA_CMD_RESTART_LSB |
//			bool_to_bit(last  && !nostop) << I2C_IC_DATA_CMD_STOP_LSB |
//			I2C_IC_DATA_CMD_CMD_BITS; // -> 1 for read
//
//		do {
//			abort_reason = i2c->hw->tx_abrt_source;
//			abort = (bool)i2c->hw->clr_tx_abrt;
//
//			if (timeout_check) {
//				timeout = timeout_check(ts);
//				abort |= timeout;
//			}
//			tight_loop_contents(); // ?
//		} while (!abort && !i2c_get_read_available(i2c));
//
//		if (timeout) {
//			// if we had a timeout, send an abort request to the hardware,
//			// so that the bus gets released
//			i2cex_abort_xfer(i2c);
//		}
//		if (abort) break;
//
//		*dst++ = (uint8_t)i2c->hw->data_cmd;
//	}
//
//	int rval;
//
//	if (abort) {
//		const int addr_noack = I2C_IC_TX_ABRT_SOURCE_ABRT_7B_ADDR_NOACK_BITS
//		                     | I2C_IC_TX_ABRT_SOURCE_ABRT_10ADDR1_NOACK_BITS
//		                     | I2C_IC_TX_ABRT_SOURCE_ABRT_10ADDR2_NOACK_BITS;
//
//		if (timeout) rval = PICO_ERROR_TIMEOUT;
//		else if (!abort_reason || (abort_reason & addr_noack))
//			rval = PICO_ERROR_GENERIC;
//		else rval = PICO_ERROR_GENERIC;
//	} else rval = byte_ctr;
//
//	i2c->restart_on_next = nostop;
//	return rval;
//}
//static inline int i2cex_write_timeout_us(i2c_inst_t* i2c, uint16_t addr, bool a10bit,
//		const uint8_t* src, size_t len, bool nostop, uint32_t timeout_us) {
//	absolute_time_t t = make_timeout_time_us(timeout_us);
//	return i2cex_write_blocking_until(i2c, addr, a10bit, src, len, nostop, t);
//}
//static inline int i2cex_read_timeout_us(i2c_inst_t* i2c, uint16_t addr, bool a10bit,
//		uint8_t* dst, size_t len, bool nostop, uint32_t timeout_us) {
//	absolute_time_t t = make_timeout_time_us(timeout_us);
//	return i2cex_read_blocking_until(i2c, addr, a10bit, dst, len, nostop, t);
//}

__attribute__((__const__))
enum ki2c_funcs i2ctu_get_func(void) {
	// TODO: 10bit addresses
	// TODO: SMBUS_EMUL_ALL => I2C_M_RECV_LEN
	// TODO: maybe also PROTOCOL_MANGLING, NOSTART
	return I2C_FUNC_I2C | I2C_FUNC_SMBUS_EMUL;
}

void i2ctu_init(void) {
	// default to 100 kHz (SDK example default so should be ok)
	//delay = 10; delay2 = 5;
	i2c_init(PINOUT_I2C_DEV, 100*1000);

	gpio_set_function(PINOUT_I2C_SCL, GPIO_FUNC_I2C);
	gpio_set_function(PINOUT_I2C_SDA, GPIO_FUNC_I2C);
	gpio_pull_up(PINOUT_I2C_SCL);
	gpio_pull_up(PINOUT_I2C_SDA);

	bi_decl(bi_2pins_with_func(PINOUT_I2C_SCL, PINOUT_I2C_SDA, GPIO_FUNC_I2C));
}

uint32_t i2ctu_set_freq(uint32_t freq, uint32_t us) {
	//delay = us;
	//delay2 = us >> 1;
	//if (!delay2) delay2 = 1;

	return i2c_set_baudrate(PINOUT_I2C_DEV, freq);
}

enum itu_status i2ctu_write(enum ki2c_flags flags, enum itu_command startstopflags,
		uint16_t addr, const uint8_t* buf, size_t len) {
	if (len == 0) {
		// do a read, that's less hazardous
		uint8_t stuff = 0;
		int rv = i2c_read_timeout_us(PINOUT_I2C_DEV, addr/*&0x7f*/, /*false,*/ &stuff, 1,
				!(startstopflags & ITU_CMD_I2C_IO_END), 1000*1000);
		if (rv < 0) return ITU_STATUS_ADDR_NAK;
		return ITU_STATUS_ADDR_ACK;
	} else {
		int rv = i2c_write_timeout_us(PINOUT_I2C_DEV, addr, /*false,*/ buf, len,
				!(startstopflags & ITU_CMD_I2C_IO_END), 1000*1000);
		if (rv < 0 || (size_t)rv < len) return ITU_STATUS_ADDR_NAK;
		return ITU_STATUS_ADDR_ACK;
	}
}
enum itu_status i2ctu_read(enum ki2c_flags flags, enum itu_command startstopflags,
		uint16_t addr, uint8_t* buf, size_t len) {
	if (len == 0) {
		uint8_t stuff = 0;
		int rv = i2c_read_timeout_us(PINOUT_I2C_DEV, addr, /*false,*/ &stuff, 1,
				!(startstopflags & ITU_CMD_I2C_IO_END), 1000*1000);
		if (rv < 0) return ITU_STATUS_ADDR_NAK;
		return ITU_STATUS_ADDR_ACK;
	} else {
		int rv = i2c_read_timeout_us(PINOUT_I2C_DEV, addr, /*false,*/ buf, len,
				!(startstopflags & ITU_CMD_I2C_IO_END), 1000*1000);
		if (rv < 0 || (size_t)rv < len) return ITU_STATUS_ADDR_NAK;
		return ITU_STATUS_ADDR_ACK;
	}
}