/*
 * cal_meas.c
 *
 *  Created on: 2023年7月19日
 *      Author: dell
 */

#include "cal_meas.h"

int Command_Parser(u8* recvbuffer, u8 len)
{
	switch (recvbuffer[0])
	{
		case 0x00:
		{
			xil_printf("[D] Match Command CPLD Read\r\n");
			if (len != CPLD_SPI_READ + 1)
			{
				xil_printf("[E] Command Length Mismatch!\r\n[I] CPLD Read Format: \"00 [CPLD] [REG Addr]\"\r\n");
			}
			u32 reg_data = 0x0;
			reg_data = CPLD_Read(recvbuffer + 1, recvbuffer + 2, reg_data);
			xil_printf("[I] CPLD #%d: REG Addr #0x%01X: Data=0x%03X\r\n",recvbuffer + 1, recvbuffer + 2, reg_data);
			break;
		}
		case 0x01:
		{
			xil_printf("[D] Match Command CPLD Write\r\n");
			if (len != CPLD_SPI_WRITE + 1)
			{
				xil_printf("[E] Command Length Mismatch!\r\n[I] CPLD Write Format: \"01 [CPLD] [REG Addr] [Data]\"\r\n");
				return XST_INVALID_PARAM;
			}
			
			xil_printf("[D] TBD!\r\n");
			break;
		}
		case 0xA0:
		{
			xil_printf("[D] Matched Command Reset\r\n");
			rst_CPLD(CPLD_U1,4);
			rst_CPLD(CPLD_U2,4);
			rst_CPLD(CPLD_U3,4);
			xil_printf("[I] CPLD RST Done\r\n");
			break;
		}
		case 0xA1:			//DC Cal
		{
			xil_printf("[D] Matched Command DC Cal\r\n");
			//Command Len Sanity Check
			if (len != DC_CMD_LEN + 1)		//Do NOT forget the command header
			{
				xil_printf("[E] Command Length Mismatch!\r\n[I] DC Cal Format: \"A1 [01-04:SLOT] [01:PMU/02:DPS] [0001-0100:Channel_Num] [01:V/02:I] [01-12:RLoad]\"\r\n");
				return XST_INVALID_PARAM;
			}
			DC_Control(recvbuffer + 1);		//Op-out the command header
			break;
		}
		case 0xA2:			//Freq Cal
		{
			xil_printf("[D] Matched Command Freq Cal\r\n");
			if (len > Freq_CMD_LEN + 1)		//Do NOT forget the command header
			{
				//TODO: ADD Freq Command Format Hint
				xil_printf("[E] Command Length Mismatch!\r\n[I] Freq Cal Format: TBD\r\n");
				return XST_INVALID_PARAM;
			}
			sw_config(recvbuffer + 1);		//Op-out the command header
			break;
		}
		
		default:
		{
			xil_printf("[E] Command Header No Match! \r\n[D] Received Command Header:0x%02X Check Command!\r\n",recvbuffer[0]);
			return XST_NO_FEATURE;
		}
		
	}
}

int DC_Control(u8* idata)
{
	//Comannd Parse Variables
	u8  u_slot       = 0x0;
	u8  u_m_fun      = 0x0;			//PMU/DPS Sel
	u16 u_ch_Num     = 0x0;			
	u8  u_vi_Sel     = 0x0;			//DMM Connection Config
	u8  u_rLoad      = 0x0;			//rLoadSel

	//CPLD Variables
	u8 reg_addr		= 0x0;
	u32 reg_data 	= 0x0;

	//Flags
	int Status = -255;
	int cmdValid = 1;

	//We already did length check, so no need to do again here

	u_slot = idata[0];										//1-4
	u_m_fun = idata[1];										//1:PMU/2:DPS
	u_ch_Num = (((u16)(idata[2])) << 8) | idata[3];
	u_vi_Sel = idata[4];										//1:V/2:I
	u_rLoad = idata[5];										//1~18: Refer to SCH/Protocol

	//Let's do some sanity check
	//Slot
	if ((u_slot > 4) || (u_slot == 0))
	{
		xil_printf("[E] Illege Slot Num check Command! (Slot:1-4)\r\n");
		cmdValid = -1;
	}
	//Dev Sel
	if ((u_m_fun > 2) || (u_m_fun == 0))
	{
		xil_printf("[E] Illege Dev Select check Command! (1:PMU/2:DPS)\r\n");
		cmdValid = -2;
	}
	//Channel
	if (u_m_fun == 1)			//PMU
	{
		
		if ((u_ch_Num < 1) || (u_ch_Num > 256))
		{
			
			xil_printf("[E] Illege Channel! (PMU Ch:0x0001-0x0100)\r\n");
			cmdValid = -2;
		}
	}
	else
	{
		if ((u_ch_Num < 1) || (u_ch_Num > 64))
		{
			
			xil_printf("[E] Illege Channel! (PMU Ch:0x0001-0x0040)\r\n");
			cmdValid = -3;
		}
	}
	
	//V/I Sel
	if ((u_vi_Sel > 2) || (u_vi_Sel == 0))
	{
		xil_printf("[E] Illege V/I Config check Command! (1:V/2:I)\r\n");
		cmdValid = -5;
	}
	//RLoad Sel
	if ((u_rLoad > 18) || (u_rLoad == 0))
	{
		xil_printf("[E] Illege RLoad Select check Command! (RLoad:0x01-0x12)\r\n");
		cmdValid = -6;
	}
	if (cmdValid != 1)
	{
		
		return XST_INVALID_PARAM;
	}

	//DEBUG Printout
	xil_printf("[D] We Got:\r\n[-]\tSlot: #%d\r\n",u_slot);
	if (u_m_fun == 1)
	{
		
		xil_printf("[-]\tDevice Select: PMU\r\n");
	}
	else
	{
		xil_printf("[-]\tDevice Select: DPS\r\n");
	}
	xil_printf("[-]\tChannel Select: #%d\r\n",u_ch_Num);
	if (u_vi_Sel == 1)
	{
		
		xil_printf("[-]\tDMM Measure Select: Voltage\r\n");
	}
	else
	{
		xil_printf("[-]\tDMM Measure Select: Current\r\n");
	}
	xil_printf("[-]\tRLoad Select: #%d\r\n",u_rLoad);
	
	//Sanity Check Done
	//Let's format the CPLD Reg Data

	//As the structure of DC regs are the same, we can format the data uniformly
	//Dev Sel
	if (u_m_fun == 0x2)
	{
		reg_data = 0x800000;			//1:DPS/0:PMU
		
	}
	//V/I Sel
	if (u_vi_Sel == 0x2)
	{
		
		reg_data = reg_data | 0x400000;	//1:I/0:V
	}
	//RLoad Sel
	reg_data = reg_data | (((u32)u_rLoad)<<17);
	//Channel Sel
	reg_data = reg_data |((u32)(u_ch_Num)<<1) | 0x1;			//0x1:Slot_EN

	//We should clear out other slot's enable flag to prevent error
	Status = 0;
	//Note here as the CPLD_Write() will smartly set the R/W bit to 1'b1, so no need to handle that here, addr is addr
	switch (u_slot)
	{
		case 1:
		{
			
			Status = Status + CPLD_Write(0x07, 0x0);
			Status = Status + CPLD_Write(0x08, 0x0);
			Status = Status + CPLD_Write(0x09, 0x0);
			reg_addr = 0x06;
			Status = Status + CPLD_Write(reg_addr, reg_data);
			break;
		}
		case 2:
		{
			Status = Status + CPLD_Write(0x06, 0x0);
			Status = Status + CPLD_Write(0x08, 0x0);
			Status = Status + CPLD_Write(0x09, 0x0);
			reg_addr = 0x07;
			Status = Status + CPLD_Write(reg_addr, reg_data);
			break;
		}
		case 3:
		{
			Status = Status + CPLD_Write(0x06, 0x0);
			Status = Status + CPLD_Write(0x07, 0x0);
			Status = Status + CPLD_Write(0x09, 0x0);
			reg_addr = 0x08;
			Status = Status + CPLD_Write(reg_addr, reg_data);
			break;
		}
		case 4:
		{
			Status = Status + CPLD_Write(0x06, 0x0);
			Status = Status + CPLD_Write(0x07, 0x0);
			Status = Status + CPLD_Write(0x08, 0x0);
			reg_addr = 0x09;
			Status = Status + CPLD_Write(reg_addr, reg_data);
			break;
		}
		default:				//You should NOT be here
		{
			xil_printf("[E] Error Reaching Invaild Slot CPLD Formatting, Check PS Code!\r\n");
			return XST_FAILURE;
		}
	}

	if (Status != XST_SUCCESS)
	{
		
		xil_printf("[E] Error Writing CPLD Reg! Check Log ABOVE!\r\n");
		return XST_FAILURE;
	}
	
	else
	{
		xil_printf("[D] CPLD Write Successfully Done!\r\n");
	}
	
	Status = CPLD_exec();

	if (Status != XST_SUCCESS)
	{
		xil_printf("[E] Error Exec CPLD ! Check Log ABOVE!\r\n");
		return XST_FAILURE;
	}

	else
	{
		xil_printf("[D] CPLD Exec Successfully Done!\r\n");
	}

	return XST_SUCCESS;
}


void  sw_config(char *slot_start)
{
		 char  slot       = (*slot_start);
		 char  m_fun      = *(slot_start+1);
		 char  ch_num_h   = *(slot_start+2);
		 char  ch_num_l   = *(slot_start+3);
		 char  volt_cur   = *(slot_start+4);
		 char  load       = *(slot_start+5);
		 char  memt_areal = *(slot_start+6);

		 u16 	 ch_num     = 0;
		 u32     ch_num_dat = 0;
		 u32     relay_ctrl = 0;
		 u32     slot_en    = 0x00000001;

		 u8      cpld_addr = 0;
		 u32     cpld_dat  = 0;
		 u8      sw_config_data[3] = {0x00,0x00,0x00};

		 ch_num = (((u16)ch_num_h)<<8) + ((u16)ch_num_l);


		 if(slot==1)
		 {
			 cpld_addr = 0x82;
		 }
		 else if(slot==2)
		 {
			 cpld_addr = 0x83;
		 }
		 else if(slot==3)
		 {
			 cpld_addr = 0x84;
		 }
		 else
		 {
			 cpld_addr = 0x85;
		 }

		 switch(ch_num)
		 {
		   case 0x0001:
		 	   {
		 		   relay_ctrl = 0x00000000;
		 		   break;
		 	   }
		   case 0x0002:
		   	   {
		   		   relay_ctrl = 0x00020000;
		   		   break;
		   	   }
		   case 0x0003:
		   	   {
		   		   relay_ctrl = 0x00200000;
		   		   break;
		   	   }
		   case 0x0004:
		   	   {
		   		   relay_ctrl = 0x00220000;
		   		   break;
		   	   }
		   case 0x0005:
		   	   {
		   		   relay_ctrl = 0x00800000;
		   		   break;
		   	   }
		   case 0x0006:
		   	   {
		   		   relay_ctrl = 0x00810000;
		   		   break;
		   	   }
		   case 0x0007:
		   	   {
		   		   relay_ctrl = 0x00A00000;
		   		   break;
		   	   }
		   case 0x0008:
		   	   {
		   		   relay_ctrl = 0x00A10000;
		   		   break;
		   	   }
		   case 0x0041:
		   	   {
		   		   relay_ctrl = 0x00000000;
		   		   break;
		   	   }
		   case 0x0042:
		   	   {
		   		   relay_ctrl = 0x00008000;
		   		   break;
		   	   }
		   case 0x0043:
		   	   {
		   		   relay_ctrl = 0x00100000;
		   		   break;
		   	   }
		   case 0x0044:
		   	   {
		   		   relay_ctrl = 0x00108000;
		   		   break;
		   	   }
		   case 0x0045:
		   	   {
		   		   relay_ctrl = 0x00800000;
		   		   break;
		   	   }
		   case 0x0046:
		   	   {
		   		   relay_ctrl = 0x00804000;
		   		   break;
		   	   }
		   case 0x0047:
		   	   {
		   		   relay_ctrl = 0x00900000;
		   		   break;
		   	   }
		   case 0x0048:
		   	   {
		   		   relay_ctrl = 0x00904000;
		   		   break;
		   	   }
		   case 0x0081:
		   	   {
		   		   relay_ctrl = 0x00000000;
		   		   break;
		   	   }
		   case 0x0082:
		   	   {
		   		   relay_ctrl = 0x00002000;
		   		   break;
		   	   }
		   case 0x0083:
		   	   {
		   		   relay_ctrl = 0x00080000;
		   		   break;
		   	   }
		   case 0x0084:
		   	   {
		   		   relay_ctrl = 0x00082000;
		   		   break;
		   	   }
		   case 0x0085:
		   	   {
		   		   relay_ctrl = 0x00400000;
		   		   break;
		   	   }
		   case 0x0086:
		   	   {
		   		   relay_ctrl = 0x00401000;
		   		   break;
		   	   }
		   case 0x0087:
		   	   {
		   		   relay_ctrl = 0x00480000;
		   		   break;
		   	   }
		   case 0x0088:
		   	   {
		   		   relay_ctrl = 0x00481000;
		   		   break;
		   	   }
		   case 0x00C1:
		   	   {
		   		   relay_ctrl = 0x00000000;
		   		   break;
		   	   }
		   case 0x00C2:
		   	   {
		   		   relay_ctrl = 0x00000800;
		   		   break;
		   	   }
		   case 0x00C3:
		   	   {
		   		   relay_ctrl = 0x00040000;
		   		   break;
		   	   }
		   case 0x00C4:
		   	   {
		   		   relay_ctrl = 0x00040800;
		   		   break;
		   	   }
		   case 0x00C5:
		   	   {
		   		   relay_ctrl = 0x00400000;
		   		   break;
		   	   }
		   case 0x00C6:
		   	   {
		   		   relay_ctrl = 0x00400400;
		   		   break;
		   	   }
		   case 0x00C7:
		   	   {
		   		   relay_ctrl = 0x00440000;
		   		   break;
		   	   }
		   case 0x00C8:
		   	   {
		   		   relay_ctrl = 0x00440400;
		   		   break;
		   	   }
		 }

		 ch_num_dat = ((u32)ch_num)<<1;
		 cpld_dat   = relay_ctrl|ch_num_dat|slot_en;

		 sw_config_data[0] = (u8)(cpld_dat>>16);
		 sw_config_data[1] = (u8)(cpld_dat>>8);
		 sw_config_data[2] = (u8)(cpld_dat);

		 xil_printf("relay_ctrl: %d\n\r", relay_ctrl);
		 xil_printf("ch_num_dat: %d\n\r", ch_num_dat);

		 xil_printf("cpld_addr: %d\n\r",  cpld_addr);
		 xil_printf("cpld_dat: %d\n\r",   cpld_dat);

		 xil_printf("sw_config_data[0]: %d\n\r", sw_config_data[0]);
		 xil_printf("sw_config_data[0]: %d\n\r", sw_config_data[1]);
		 xil_printf("sw_config_data[0]: %d\n\r", sw_config_data[2]);

		 spi_write_onewire(cpld_addr, sw_config_data, dat_width, CPLD_U1);
		 spi_write_onewire(cpld_addr, sw_config_data, dat_width, CPLD_U2);
		 spi_write_onewire(cpld_addr, sw_config_data, dat_width, CPLD_U3);

		 sw_config_data[0] = 0x00;
		 sw_config_data[1] = 0x00;
		 sw_config_data[2] = 0x01;
		 spi_write_onewire(0x81, sw_config_data, dat_width, CPLD_U1);
		 spi_write_onewire(0x81, sw_config_data, dat_width, CPLD_U2);
		 spi_write_onewire(0x81, sw_config_data, dat_width, CPLD_U3);

		 slot_en   = 0x00000000;
		 cpld_dat   = relay_ctrl|ch_num_dat|slot_en;

		 sw_config_data[0] = (u8)(cpld_dat>>16);
		 sw_config_data[1] = (u8)(cpld_dat>>8);
		 sw_config_data[2] = (u8)(cpld_dat);

		 spi_write_onewire(cpld_addr, sw_config_data, dat_width, CPLD_U1);
		 spi_write_onewire(cpld_addr, sw_config_data, dat_width, CPLD_U2);
		 spi_write_onewire(cpld_addr, sw_config_data, dat_width, CPLD_U3);
}



