#include <string.h>
#include "bsp/gpio.h"
#include "bsp/ml5238.h"
#include "bsp/uart.h"
#include "bsp/gd32_adc.h"
#include "bsp/cs1180.h"
#include "nv_storage.h"
#include "pcba_test.h"
static void pcba_test_timer_handler(shark_timer_t *timer);
static shark_timer_t pcba_test_timer = {.handler = pcba_test_timer_handler};
static int pcba_test_item = -1;
static int powerdown_flags = 0;

const rcu_periph_enum shark_gpio_rcus[] = { RCU_GPIOA, RCU_GPIOB, RCU_GPIOC, RCU_GPIOD, RCU_GPIOF, RCU_GPIOF };
const u32 shark_gpio_ports[] = { GPIOA, GPIOB, GPIOC, GPIOD, GPIOF, GPIOF };

static void pcba_test_timer_handler(shark_timer_t *timer) {
	if (pcba_test_item == 1) {
		UART0_IR_EN(1);
	}
	pcba_test_item = -1;
}
int pcba_test(uint8_t *data, int len, uint8_t *response){
	uint8_t *payload = data + 2;
	int resp_len = 0;
	response[resp_len ++] = 0xFE;
	response[resp_len ++] = 0xFE;
	response[resp_len ++] = 0; //len
	//response[resp_len ++] = payload[0];
	response[resp_len ++] = payload[1];
	uint8_t cmd = payload[1];
	if (cmd == 0x00) {
		response[resp_len ++] = ml5238_noop_register_rw(payload[2]);
	}else if (cmd == 0x01) {
		response[resp_len ++] = (uint8_t)nv_read_write_test();
	}else if (cmd == 0x02){
		for (int i = 0; i < PACK_TEMPS_NUM; i++){
			response[resp_len ++] = (uint8_t)get_pack_temperature(i) + 40;
		}
	}else if (cmd == 0x03) {
		response[resp_len ++] = io_state()->aux_lock_detect?0:1;
	}else if (cmd == 0x04) {
		LED1_ON(0);
		LED3_ON(0);
		LED0_ON(1);
		LED2_ON(1);
		LED4_ON(1);
		response[resp_len ++] = 0;
	}else if (cmd == 0x05) {
		LED0_ON(0);
		LED2_ON(0);
		LED4_ON(0);		
		LED1_ON(1);
		LED3_ON(1);
		response[resp_len ++] = 0;
	}else if (cmd == 0x06) {
		LED1_ON(0);
		LED3_ON(0);
		charger_open(0);
		discharger_open(0);
		start_aux_power(0);
		response[resp_len ++] = 0;
	}else if (cmd == 0x07) {
		start_aux_power(1);
		io_state()->aux_lock_irq = 0;
		response[resp_len ++] = 0;
	}else if (cmd == 0x08) {
		response[resp_len ++] = io_state()->aux_lock_irq;
	}else if (cmd == 0x09) {
		start_aux_power(0);
		response[resp_len ++] = 0;
	}else if (cmd == 0x0a) {
		ml5238_enable_load_detect(1);
		delay_us(1000);
		response[resp_len ++] = !ml5238_is_load_disconnect();
	}else if (cmd == 0xb) {
		charger_open(0);
		discharger_open(0);
		start_aux_power(0);
		delay_us(1000);
		response[resp_len ++] = io_state()->charger_detect_irq;
	}else if (cmd == 0xc) {
		charger_open(0);
		discharger_open(0);
		start_aux_power(0);
		response[resp_len ++] = IS_CHARGER_IN();
	}else if (cmd == 0xd) {
		response[resp_len ++] = io_state()->aux_lock_detect;
	}else if (cmd == 0xe) {
		charger_open(1);
		discharger_open(1);
		start_aux_power(0);
		response[resp_len ++] = 0;
		powerdown_flags = 0xF;
		system_power_down();
	}else if (cmd == 0xf){
		response[resp_len ++] = !IS_HALL1_DETECTED();
		response[resp_len ++] = !IS_HALL2_DETECTED();
	}else if (cmd == 0x10) {
		if (payload[2] >= 3) {
			charger_open(1);
			discharger_open(1);
		}else if (payload[2] == 1){
			charger_open(0);
			discharger_open(1);
		}else{
			charger_open(1);
			discharger_open(0);
		}
		response[resp_len ++] = 0;
	}else if (cmd == 0x11) {
		response[resp_len ++] = ml5238_is_discharging() | ml5238_is_charging()<<1;
	}else if (cmd == 0x22) {
		uint16_t health = (uint16_t)bms_health()->i_status;
		response[resp_len ++] = AUX_VOL_IS_OPEN();
		response[resp_len ++] = health;
		response[resp_len ++] = health >> 8;
	}else if (cmd == 0xa1) {
		charger_open(0);
		discharger_open(0);
		start_aux_power(0);
		response[resp_len ++] = 0;
	}else if (cmd == 0x12) {
		uint16_t aux_power_adc = (uint16_t)adc_sample_avg(ADC_CHAN_AUX_CURR, 10);
		response[resp_len ++] = aux_power_adc;
		response[resp_len ++] = aux_power_adc >> 8;		
	}else if (cmd == 0x13) {
		//shark_uart_deinit(SHARK_UART1);
		//rcu_periph_clock_enable(RCU_GPIOA);
		gpio_mode_output(GPIOA, GPIO_PUPD_NONE, GPIO_OTYPE_PP, GPIO_OSPEED_10MHZ, GPIO_PIN_2);
		gpio_mode_input(GPIOA, GPIO_PUPD_NONE, GPIO_PIN_3);
		gpio_bit_set(GPIOA, GPIO_PIN_2);
		response[resp_len ++] = gpio_input_bit_get(GPIOA, GPIO_PIN_3);
	}else if (cmd == 0x16) {
		gpio_bit_reset(GPIOA, GPIO_PIN_2);
		response[resp_len ++] = gpio_input_bit_get(GPIOA, GPIO_PIN_3);
	}else if (cmd == 0x14) {
		UART0_IR_EN(0);
		response[resp_len ++] = gpio_output_bit_get(GPIOC,GPIO_PIN_14);
		pcba_test_item = 1;
		shark_timer_post(&pcba_test_timer, 500);
	}else if (cmd == 0x15) {
		UART0_IR_EN(1);
		pcba_test_item = -1;
		shark_timer_cancel(&pcba_test_timer);
		response[resp_len ++] = gpio_output_bit_get(GPIOC,GPIO_PIN_14);
	}else if (cmd == 0x17) {
		uint8_t cell_index = 0;
		for (int i = 0; i < CELLS_NUM; i++){
			if ((measure_value()->cell_vol[i] < 3000) || (measure_value()->cell_vol[i] > 3400)) {
				cell_index = i + 1;
				break;
			}
		}
		response[resp_len ++] = cell_index;
	}else if (cmd == 0x18) {
		uint8_t result = 1;
		response[resp_len ++] = payload[2];
		if (payload[2] == 1) {
			charger_open(1);
			discharger_open(1);
			result = 0;
		}else if (payload[2] == 2){
			if (measure_value()->load_current >= -5300 && measure_value()->load_current <= -4700){
				result = 0;
			}
		}else if (payload[2] == 3) {
			if (bms_health()->load_current_short) {
				result = 0;
			}
		}else if (payload[2] == 4) {
			charger_open(0);
			discharger_open(0);
			result = 0;			
		}
		response[resp_len ++] = result;
	}else if (cmd == 0x20) {
		response[resp_len ++] = payload[2];
		if (payload[2] == 1) {
			response[resp_len ++] = cs1180_is_ready();
		}else if (payload[2] == 2) {
			charger_open(1);
			discharger_open(1);
			response[resp_len ++] = 0;
		}else if (payload[2] == 3) {
			memcpy(response + resp_len, &measure_value()->load_current, 4);
			resp_len += 4;
		}
	}else if (cmd == 0x21) {
		response[resp_len ++] = powerdown_flags;
	}else if (cmd == 0xC9) {
		if (payload[2] == 0) {
			DIANOCTIC_LED(0);
		}else {
			DIANOCTIC_LED(1);
		}
	} else if (cmd == 0xE1 && len > 4) { // gpio control
		u8 index = (data[4] >> 4) & 0x0F;
		u32 pin = 1 << (data[4] & 0x0F);
		u32 port = shark_gpio_ports[index];
		rcu_periph_enum rcu = shark_gpio_rcus[index];

		rcu_periph_clock_enable(rcu);
		response[resp_len++] = data[4];

		if (len < 6 || data[5] == 0x00) { // input get
			response[resp_len++] = gpio_input_bit_get(port, pin);
		} else if (data[5] == 0x01) { // output set and get
			if (len > 6) {
				if (data[6] != 0x00) {
					gpio_bit_set(port, pin);
				} else {
					gpio_bit_reset(port, pin);
				}
			}

			response[resp_len++] = gpio_output_bit_get(port, pin);
		} else if (data[5] == 0x02) { // input get
			gpio_mode_input(port, GPIO_PUPD_NONE, pin);
			response[resp_len++] = gpio_input_bit_get(port, pin);
		} else if (data[5] == 0x03 && len > 6) { // output set and get
			if (data[6] != 0x00) {
				gpio_bit_set(port, pin);
			} else {
				gpio_bit_reset(port, pin);
			}

			gpio_mode_output(port, GPIO_PUPD_NONE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, pin);
			response[resp_len++] = gpio_output_bit_get(port, pin);
		} else if (data[5] == 0x04 && len > 6) { // input get
			gpio_mode_input(port, data[6], pin);
			response[resp_len++] = gpio_input_bit_get(port, pin);
		} else if (data[5] == 0x05 && len > 8) { // output set
			if (data[6] != 0x00) {
				gpio_bit_set(port, pin);
			} else {
				gpio_bit_reset(port, pin);
			}

			gpio_mode_output(port, data[7], data[8], GPIO_OSPEED_50MHZ, pin);
			response[resp_len++] = gpio_output_bit_get(port, pin);
		} else {
			response[resp_len++] = 0xFF;
		}
	}

	response[2] = resp_len - 2;

	return resp_len;
}