#include "mks_wifi.h"
#ifdef MKS_WIFI

#include "../../lcd/ultralcd.h"
#include "mks_wifi_sd.h"
#include "mks_test_sdio.h"

volatile uint8_t mks_in_buffer[MKS_IN_BUFF_SIZE];
uint8_t mks_out_buffer[MKS_OUT_BUFF_SIZE];

volatile uint8_t esp_packet[MKS_TOTAL_PACKET_SIZE];


void mks_wifi_init(void){

	SERIAL_ECHO_MSG("Init MKS WIFI");	
    DEBUG("Init MKS WIFI");
	
	SET_OUTPUT(MKS_WIFI_IO0);
	WRITE(MKS_WIFI_IO0, HIGH);

	SET_OUTPUT(MKS_WIFI_IO4);
	WRITE(MKS_WIFI_IO4, HIGH);

	SET_OUTPUT(MKS_WIFI_IO_RST);
	WRITE(MKS_WIFI_IO_RST, LOW);

	ui.set_status((const char *)"WIFI: waiting... ",false);

	safe_delay(200);	
	WRITE(MKS_WIFI_IO_RST, HIGH);
	
	safe_delay(200);	
	WRITE(MKS_WIFI_IO4, LOW);

	#ifdef SDIO_TEST_AT_STARTUP
	mks_test_sdio();
	#endif
	
	#ifdef LIST_FILES_AT_STARTUP
	mks_wifi_sd_deinit(); 
	mks_wifi_sd_init();  
	mks_wifi_sd_ls();
	mks_wifi_sd_deinit(); 
	#endif

}


void mks_wifi_set_param(void){
	uint32_t packet_size;
	ESP_PROTOC_FRAME esp_frame;


	uint32_t ap_len = strlen((const char *)MKS_WIFI_SSID);
	uint32_t key_len = strlen((const char *)MKS_WIFI_KEY);


	memset(mks_out_buffer, 0, MKS_OUT_BUFF_SIZE);

	mks_out_buffer[0] = WIFI_MODE_STA;

	mks_out_buffer[1] = ap_len;
	strncpy((char *)&mks_out_buffer[2], (const char *)MKS_WIFI_SSID, ap_len);

	mks_out_buffer[2+ap_len] = key_len;
	strncpy((char *)&mks_out_buffer[2 + ap_len + 1], (const char *)MKS_WIFI_KEY, key_len);

	esp_frame.type=ESP_TYPE_NET;
	esp_frame.dataLen= 2 + ap_len + key_len + 1;
	esp_frame.data=mks_out_buffer;
	packet_size=mks_wifi_build_packet((uint8_t *)esp_packet,&esp_frame);

	if(packet_size > 8){ //4 байта заголовка + 2 байта длины + хвост + название сети и пароль
		//выпихнуть в uart
		mks_wifi_send((uint8_t *)esp_packet, packet_size);
	};
}

/*
Получает данные из всех функций, как только
есть перевод строки 0x0A, формирует пакет для
ESP и отправляет
*/
void mks_wifi_out_add(uint8_t *data, uint32_t size){
	static uint32_t line_index=0;
	uint32_t packet_size;
	ESP_PROTOC_FRAME esp_frame;

	while (size--){
		if(*data == 0x0a){
			//Перевод строки => сформировать пакет, отправить, сбросить индекс
			esp_frame.type=ESP_TYPE_FILE_FIRST; //Название типа из прошивки MKS. Смысла не имееет.
			esp_frame.dataLen=strnlen((char *)mks_out_buffer,MKS_OUT_BUFF_SIZE);
			esp_frame.data=mks_out_buffer;
			packet_size=mks_wifi_build_packet((uint8_t *)esp_packet,&esp_frame);

			if(packet_size == 0){
				ERROR("Build packet failed");
				line_index=0;
				memset(mks_out_buffer,0,MKS_OUT_BUFF_SIZE);
				break;
			}
			//выпихнуть в uart
			mks_wifi_send((uint8_t *)esp_packet, packet_size);
			//очистить буфер
			memset(mks_out_buffer,0,MKS_OUT_BUFF_SIZE);
			//сбросить индекс
			line_index=0;
		}else{
			//писать в буфер			
			mks_out_buffer[line_index++]=*data++;
		}

		if(line_index >= MKS_OUT_BUFF_SIZE){
			ERROR("Max line size");
			line_index=0;
			memset(mks_out_buffer,0,MKS_OUT_BUFF_SIZE);
			break;
		}
	}
}

uint8_t mks_wifi_input(uint8_t data){
	ESP_PROTOC_FRAME esp_frame;
	#ifdef MKS_WIFI_ENABLED_WIFI_CONFIG 
	static uint8_t get_packet_from_esp=0;
	#endif
	static uint8_t packet_start_flag=0;
	static uint8_t packet_type=0;
	static uint16_t packet_index=0;
	static uint16_t payload_size=ESP_PACKET_DATA_MAX_SIZE;
	uint8_t ret_val=1;

	//Не отдавать данные в очередь команд, если идет печать
	if (CardReader::isPrinting()){
		DEBUG("No input while printing");
		return 1;
	}	
	
	if(data == ESP_PROTOC_HEAD){
		payload_size = ESP_PACKET_DATA_MAX_SIZE;
		packet_start_flag=1;
		packet_index=0;
		memset((uint8_t*)mks_in_buffer,0,MKS_IN_BUFF_SIZE);
	}else if(!packet_start_flag){
		DEBUG("Byte not in packet %0X %c",data,data);
	}

	if(packet_start_flag){
		mks_in_buffer[packet_index]=data;
	}

	if(packet_index == 1){
		packet_type = mks_in_buffer[1];
	}

	if(packet_index == 3){
		payload_size = uint16_t(mks_in_buffer[3] << 8) | mks_in_buffer[2];

		if(payload_size > ESP_PACKET_DATA_MAX_SIZE){
			ERROR("Payload size too big");
			packet_start_flag=0;
			packet_index=0;
			memset((uint8_t*)mks_in_buffer,0,MKS_IN_BUFF_SIZE);
			return 1;
		}

	}

	if( (packet_index >= (payload_size+4)) || (packet_index >= ESP_PACKET_DATA_MAX_SIZE) ){

		if(mks_wifi_check_packet((uint8_t *)mks_in_buffer)){
			ERROR("Packet check failed");
			packet_start_flag=0;
			packet_index=0;
			return 1;
		}

		esp_frame.type = packet_type;
		esp_frame.dataLen = payload_size;
		esp_frame.data = (uint8_t*)&mks_in_buffer[4];

		mks_wifi_parse_packet(&esp_frame);

		#ifdef MKS_WIFI_ENABLED_WIFI_CONFIG 
		if(!get_packet_from_esp){
			DEBUG("Fisrt packet from ESP, send config");
		
			mks_wifi_set_param();
			get_packet_from_esp=1;
		}
		#endif
		packet_start_flag=0;
		packet_index=0;
	}

	/* Если в пакете G-Сode, отдаем payload дальше в обработчик марлина */
	if((packet_type == ESP_TYPE_GCODE) && 
	   (packet_index >= 4) && 
	   (packet_index < payload_size+5) 
	  ){

		if(!check_char_allowed(data)){
			ret_val=0;
		}else{
			ERROR("Char not allowed: %0X %c",data,data);
			packet_start_flag=0;
			packet_index=0;
			return 1;
		}
		
	}

	if(packet_start_flag){
		packet_index++;
	}

	return ret_val;
}

/*
Проверяет, что символы из текстового диапазона
для G-code команд
*/
uint8_t check_char_allowed(char data){

	if( data == 0x0a || data == 0x0d){
		return 0;
	}

	if( (data >= 0x20) && (data <= 0x7E) ){
		return 0;
	}

	return 1;

}

/*
Проверяет пакет на корректность:
наличие заголовка
наличие "хвоста"
длина пакета
*/
uint8_t mks_wifi_check_packet(uint8_t *in_data){
	uint16_t payload_size;
	uint16_t tail_index;

	if(in_data[0] != ESP_PROTOC_HEAD){
		ERROR("Packet head mismatch");
		return 1;
	}

	payload_size = uint16_t(in_data[3] << 8) | in_data[2];
	
	if(payload_size > ESP_PACKET_DATA_MAX_SIZE){
		ERROR("Payload size mismatch");
		return 1;
	}
	
	tail_index = payload_size + 4;

	if(in_data[tail_index] != ESP_PROTOC_TAIL ){
		ERROR("Packet tail mismatch");
		return 1;
	}

	return 0;
}


void mks_wifi_parse_packet(ESP_PROTOC_FRAME *packet){
	static uint8_t show_ip_once=0;
	char str[100];

	switch(packet->type){
		case ESP_TYPE_NET:
			memset(str,0,100);
			if(packet->data[6] == ESP_NET_WIFI_CONNECTED){
				if(show_ip_once==0){
					show_ip_once=1;
					sprintf(str,"IP %d.%d.%d.%d",packet->data[0],packet->data[1],packet->data[2],packet->data[3]);
					ui.set_status((const char *)str,true);
					SERIAL_ECHO_START();
					SERIAL_ECHOLN((char*)str);	

					//Вывод имени сети
					
					if(packet->data[8] < 100){
						memcpy(str,&packet->data[9],packet->data[8]); 
						str[packet->data[8]]=0;
						SERIAL_ECHO_START();
						SERIAL_ECHO("WIFI: ");
						SERIAL_ECHOLN((char*)str);
					}else{
						DEBUG("Network NAME too long");
					}
				}
				DEBUG("[Net] connected, IP: %d.%d.%d.%d",packet->data[0],packet->data[1],packet->data[2],packet->data[3]);
			}else if(packet->data[6] == ESP_NET_WIFI_EXCEPTION){
				DEBUG("[Net] wifi exeption");
			}else{
				DEBUG("[Net] wifi not config");
			}
			break;
		case ESP_TYPE_GCODE:
			break;
		case ESP_TYPE_FILE_FIRST:
				DEBUG("[FILE_FIRST]");
				//Передача файла останавливает все процессы, 
				//поэтому печать в этот момент не возможна.
				if (!CardReader::isPrinting()){
					mks_wifi_start_file_upload(packet);
				}
			break;
		case ESP_TYPE_FILE_FRAGMENT:
				DEBUG("[FILE_FRAGMENT]");
			break;
		case ESP_TYPE_WIFI_LIST:
			DEBUG("[WIFI_LIST]");
			break;
		default:
			DEBUG("[Unkn]");
		 	break;

	}

}

void mks_wifi_print_var(uint8_t count, ...){
	va_list args;
	uint8_t data;

	va_start(args, count);

    while (count--) {
        data = va_arg(args, unsigned);
		mks_wifi_out_add(&data, 1);
    }
    va_end(args);
}


void mks_wifi_print(const char *s){
	mks_wifi_out_add((uint8_t *)s, strnlen((char *)s,ESP_PACKET_DATA_MAX_SIZE));
}

void mks_wifi_print(int i){
	char str[100];

	sprintf(str,"%d",i);
	mks_wifi_out_add((uint8_t *)str, strnlen((char *)str,ESP_PACKET_DATA_MAX_SIZE));
}


void mks_wifi_println(const char *s){
	mks_wifi_out_add((uint8_t *)s, strnlen((char *)s,ESP_PACKET_DATA_MAX_SIZE));
}

void mks_wifi_println(float f){
	char str[100];

	sprintf(str,"%ld\n",(uint32_t)f);
	mks_wifi_out_add((uint8_t *)str, strnlen((char *)str,ESP_PACKET_DATA_MAX_SIZE));
}

uint16_t mks_wifi_build_packet(uint8_t *packet, ESP_PROTOC_FRAME *esp_frame){
	uint16_t packet_size=0;

	memset(packet,0,MKS_TOTAL_PACKET_SIZE);
	packet[0] = ESP_PROTOC_HEAD;
	packet[1] = esp_frame->type;

	for(uint32_t i=0; i < esp_frame->dataLen; i++){
		packet[i+4]=esp_frame->data[i]; //4 байта заголовка отступить
	}

	packet_size = esp_frame->dataLen + 4;

	if(packet_size > MKS_TOTAL_PACKET_SIZE){
		ERROR("ESP packet too big");
		return 0;
	}

	if(esp_frame->type != ESP_TYPE_NET){
		packet[packet_size++] = 0x0d;
		packet[packet_size++] = 0x0a; 
		esp_frame->dataLen = esp_frame->dataLen + 2; //Два байта на 0x0d 0x0a
	}
	
	*((uint16_t *)&packet[2]) = esp_frame->dataLen;

	packet[packet_size] = ESP_PROTOC_TAIL;
	return packet_size;
}


void mks_wifi_send(uint8_t *packet, uint16_t size){
	//safe_delay(10);
	for( uint32_t i=0; i < (uint32_t)(size+1); i++){
		while(MYSERIAL1.availableForWrite()==0){
			safe_delay(10);				
		}
		MYSERIAL1.write(packet[i]);
	}
}

#endif