#include "port.h"
#include "common.h"
#include "timer.h"
#include "environment.h"
#include "system_setup.h"
#include "hardware.h"
#include <csl_intc.h>
//#define IOB_TEST

near unsigned short InputPortValue[PORT_LINE_NUMBER];
near unsigned short OutputPortValue[PORT_LINE_NUMBER];
near unsigned short AnalogOutputValue[4];
volatile unsigned short *OutputPortAddr[PORT_LINE_NUMBER];	

unsigned char POPMap[MAX_PORT_COUNT];		// POP(Physical OP) to System Port Map;
unsigned char PIPMap[MAX_PORT_COUNT];		// PIP(Physical IP) to System Port Map;

unsigned short InPortIndex[MAX_PORT_COUNT];
unsigned short InPortMask[MAX_PORT_COUNT];
unsigned short OutPortIndex[MAX_PORT_COUNT];
unsigned short OutPortMask[MAX_PORT_COUNT];

unsigned short PLCControlValue;

#define TARGET_MACHINE_REV			(('D' << 24) | ('5' << 16) | (500))

near unsigned short InputFilter[PORT_LINE_NUMBER][3];		// address_count * filter count
near unsigned short EncoderValue[11];
int PortTestOn=0;

int INVERTER_1_SPEED_AO_INDEX =0;
int INVERTER_2_SPEED_AO_INDEX =1;
int VF_SPEED_AO_INDEX=2;
int VF_HOPPER_SPPED_AO_INDEX = 2;
int PRESSURE_REGULATOR_AO_INDEX=3;


TPortMapInfo PortMapInfo;
PortStatusInfo portInfo;

int ThreeDSensorStatus[2];

void PORT_PLC_OFF(void);
void PORT_PLC_ON(void);
void PORT_SetInitialState(void);

void PORT_InitPort(void)
{
	int portIndex;
	int portMapValid, size;
	TErrorCode ec;

	memset(&InputPortValue, 0x00, sizeof(InputPortValue));
	memset(&OutputPortValue, 0x00, sizeof(OutputPortValue));
	memset(&AnalogOutputValue, 0x00, sizeof(AnalogOutputValue));
	memset(&InputFilter, 0x00, sizeof(InputFilter));
	memset(&portInfo, 0x00, sizeof(PortStatusInfo));
	memset(&ThreeDSensorStatus, 0, sizeof(int) * 2);


// make physical to system port map (depends on PIO board)
	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++) // IP0~IP32 are mapped to SIP0~SIP32
	{
		PIPMap[portIndex] = portIndex;		
	}
	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++) // OP0~OP63 are mapped to SOP0~SOP63
	{
		POPMap[portIndex] = portIndex;		
	}
	
	ec = ReadFromFlash(PID_PORT_MAP, (unsigned char *)&PortMapInfo, sizeof(TPortMapInfo), &size);
	if (ec != ERROR_CODE_NONE || size != sizeof(TPortMapInfo) || PortMapInfo.MachineRev != TARGET_MACHINE_REV)
	{
		// make all port disable
		for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
		{
			PortMapInfo.VIPMap[portIndex] = PIP_NOT_EXIST;
		}
		for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
		{
			PortMapInfo.VOPMap[portIndex] = POP_NOT_EXIST;
		}
		ErrorUnit_Put(BoardID, ERROR_TYPE_CRITICAL, ERROR_CODE_ENVIRONMENT_NOT_MATCH, 4,4,4);
		portMapValid = 0;
	}
	else
	{
		portMapValid = 1;
		//ErrorUnit_Put(BoardID, ERROR_TYPE_CRITICAL, ERROR_CODE_ENVIRONMENT_NOT_MATCH, 4,4,1);
	}

// make initial logical(virtual) to system port map
	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
	{
		if (PortMapInfo.VIPMap[portIndex] == PIP_NOT_EXIST || PortMapInfo.VIPMap[portIndex] >= MAX_PORT_COUNT)
		{
			InPortIndex[portIndex] = 0;
			InPortMask[portIndex] = 0;	// mask '0' means this VIP is not working
		}
		else
		{
			InPortIndex[portIndex] = PIPMap[PortMapInfo.VIPMap[portIndex]] / 16;		// map chaining (virtual -> phisical -> system)
			InPortMask[portIndex] = (0x0001 << (PIPMap[PortMapInfo.VIPMap[portIndex]] % 16));
		}
	}

	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
	{
		if (PortMapInfo.VOPMap[portIndex] == POP_NOT_EXIST || PortMapInfo.VOPMap[portIndex] >= MAX_PORT_COUNT )
		{
			OutPortIndex[portIndex] = 0;
			OutPortMask[portIndex] = 0;
		}
		else
		{
			OutPortIndex[portIndex] = POPMap[PortMapInfo.VOPMap[portIndex]] / 16;		// map chaining (virtual -> phisical -> system)
			OutPortMask[portIndex] = (0x0001 << (POPMap[PortMapInfo.VOPMap[portIndex]] % 16));
		}
	}

	OutputPortAddr[0] = OUTPUT_PORT_0; // 0  ~ 15
	OutputPortAddr[1] = OUTPUT_PORT_1; // 16 ~ 31
	OutputPortAddr[2] = OUTPUT_PORT_2; // 32 ~ 47
	OutputPortAddr[3] = OUTPUT_PORT_3; // 48 ~ 63
	OutputPortAddr[4] = OUTPUT_PORT_4; // 64 ~ 79
	OutputPortAddr[5] = OUTPUT_PORT_5; // 80 ~ 95


	if (portMapValid)
	{
		// init port values before power on
		PORT_SetInitialState();
		PORT_PLC_ON();
	}
}

void PORT_PLC_OFF(void)
{
	PLCControlValue = 0;
}

void PORT_PLC_ON(void)
{
	PLCControlValue = 2;
}

int PORT_ReadInPort(unsigned char vipIndex)
{
	return ((InputPortValue[InPortIndex[vipIndex]] & InPortMask[vipIndex]) != 0);
}

int PORT_ReadOutPort(unsigned char vopIndex)
{
	return ((OutputPortValue[OutPortIndex[vopIndex]] & OutPortMask[vopIndex]) != 0);
}

void PORT_ModifyAOValue(unsigned char aoIndex, unsigned short value)
{
	AnalogOutputValue[aoIndex] = value;
}

void PORT_ModifyOutPort(unsigned char vopIndex, unsigned char on)
{
	if (on)
	{
		OutputPortValue[OutPortIndex[vopIndex]] |= OutPortMask[vopIndex];
	}
	else
	{
		OutputPortValue[OutPortIndex[vopIndex]] &= (~OutPortMask[vopIndex]);
	}
}

void PORT_ModifyPhysicalOutPort(unsigned char popIndex, unsigned char on)
{
	if (popIndex < MAX_PORT_COUNT)
	{
		if (on)
		{
			OutputPortValue[POPMap[popIndex] / 16] |= (0x0001 << (POPMap[popIndex] % 16));
		}
		else
		{
			OutputPortValue[POPMap[popIndex] / 16] &= (~(0x0001 << (POPMap[popIndex] % 16)));
		}
	}
}


void PORT_ChangeOutPort(unsigned char vopIndex, unsigned char on)
{
	register unsigned char portIndex;

	portIndex = OutPortIndex[vopIndex];
	if (on)
	{
		OutputPortValue[portIndex] |= OutPortMask[vopIndex];
	}
	else
	{
		OutputPortValue[portIndex] &= (~OutPortMask[vopIndex]);
	}
	*OutputPortAddr[portIndex] = OutputPortValue[portIndex];

}

void PORT_ModifyOutPort_Value(int portIndex, unsigned short value, unsigned short mask)
{
	// to do: interlock needed
	OutputPortValue[portIndex] = (OutputPortValue[portIndex] & ~mask) | (value & mask);
}

void PORT_ChangeOutput_Value(int portIndex, unsigned short value, unsigned short mask)
{
	OutputPortValue[portIndex] = (OutputPortValue[portIndex] & ~mask) | (value & mask);
	*OutputPortAddr[portIndex] = OutputPortValue[portIndex];
}

void PORT_Read(void)
{
	//0~15 Port Read
	InputFilter[0][0] = ~*INPUT_PORT_0;
	InputFilter[0][1] = ~*INPUT_PORT_0;
	if(InputFilter[0][0] == InputFilter[0][1]) 
	{
		InputPortValue[0] = InputFilter[0][1];
	}

	//16~31 Port Read
	InputFilter[1][0] = ~*INPUT_PORT_1; 
	InputFilter[1][1] = ~*INPUT_PORT_1;
	if(InputFilter[1][0] == InputFilter[1][1]) 
	{
		InputPortValue[1] = InputFilter[1][1];
	}

	//32~47 Port Read
	InputFilter[2][0] = ~*INPUT_PORT_2;
	InputFilter[2][1] = ~*INPUT_PORT_2;
	if(InputFilter[2][0] == InputFilter[2][1])
	{
		InputPortValue[2] = InputFilter[2][1];
	}

	//47~63 Port Read
	InputFilter[3][0] = ~*INPUT_PORT_3;
	InputFilter[3][1] = ~*INPUT_PORT_3;
	if(InputFilter[3][0] == InputFilter[3][1])
	{
		InputPortValue[3] = InputFilter[3][1];
	}
	//63~79 Port Read
	InputPortValue[4] = 0x00;
	//80~95 Port Read
	InputFilter[5][0] = ~*INPUT_PORT_5;
	InputFilter[5][1] = ~*INPUT_PORT_5;
	if(InputFilter[5][0] == InputFilter[5][1])
	{
		InputPortValue[5] = InputFilter[5][1];
	}
	
}

void PORT_Write()
{
	*PLC_CONTROL = PLCControlValue;
	
	*ANALOG_OUTPUT_0 = AnalogOutputValue[0];
	*ANALOG_OUTPUT_1 = AnalogOutputValue[1];
	*ANALOG_OUTPUT_2 = AnalogOutputValue[2];
	*ANALOG_OUTPUT_3 = AnalogOutputValue[3];
	
	*OutputPortAddr[0] = OutputPortValue[0];
	*OutputPortAddr[1] = OutputPortValue[1];
	*OutputPortAddr[2] = OutputPortValue[2];
	*OutputPortAddr[3] = OutputPortValue[3];
//	*OutputPortAddr[4] = OutputPortValue[4];
	*OutputPortAddr[5] = OutputPortValue[5];
	// analog value write
}

void PORT_Write_50us()
{
	*OutputPortAddr[4] = OutputPortValue[4];
}

void Encoder_Read(void)
{
	EncoderValue[0] = *ENCODER_0;
	EncoderValue[1] = *ENCODER_1;
	EncoderValue[2] = *ENCODER_2;
	EncoderValue[3] = *ENCODER_3;
	EncoderValue[4] = *ENCODER_4;
	EncoderValue[5] = *ENCODER_8;
	EncoderValue[6] = *SERVO_MOTOR_0_ENCODER;
	EncoderValue[7] = *SERVO_MOTOR_1_ENCODER;

	if(MachineSetupData.MachineType == MACHINE_TYPE_SELMA_ADVANCED || MachineSetupData.MachineType == MACHINE_TYPE_LIMA_ADVANCED)
	{
		EncoderValue[8] = *SERVO_MOTOR_2_ENCODER;
		EncoderValue[9] = *SERVO_MOTOR_3_ENCODER;
		EncoderValue[10] = *SERVO_MOTOR_4_ENCODER;
	}	
	else
	{
		EncoderValue[8] = *SERVO_MOTOR_3_ENCODER;
		EncoderValue[9] = *SERVO_MOTOR_4_ENCODER;
		EncoderValue[10] = NULL;	
	}

}

void PORT_SetInitialState(void)
{
	PORT_Write();
	PORT_Write_50us();
}


void PORT_McBSP_GateSelect(int gateIndex)
{	
	BIT16_SET(OutputPortValue[2], SERVO_MCBSP_GATE, 1 << gateIndex);
	*OutputPortAddr[2] = OutputPortValue[2];
}

void PORT_Setup(unsigned char *portMapData)
{
	int portIndex;
	unsigned char *inputPortMap = portMapData;
	unsigned char *outputPortMap = portMapData + 96;//64;

	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
	{
		if (inputPortMap[portIndex] > MAX_PORT_COUNT)
		{
			PortMapInfo.VIPMap[portIndex] = PIP_NOT_EXIST;
		}
		else
		{
			PortMapInfo.VIPMap[portIndex] = inputPortMap[portIndex];
		}
	}

	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
	{
		if (outputPortMap[portIndex] > MAX_PORT_COUNT)
		{
			PortMapInfo.VOPMap[portIndex] = POP_NOT_EXIST;
		}
		else
		{
			PortMapInfo.VOPMap[portIndex] = outputPortMap[portIndex];
		}
	}

	PortMapInfo.MachineRev = TARGET_MACHINE_REV;
	WriteToFlash(PID_PORT_MAP, (unsigned char *)&PortMapInfo, sizeof(TPortMapInfo), 0);
	PORT_InitPort();
}

void Port_CheckStatus(TPortStatus *portStatus)
{
	int portIndex;
	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
	{
		if (PortMapInfo.VIPMap[portIndex] == PIP_NOT_EXIST)
		{
			portStatus->IPStatus[portIndex] = 0;
		}
		else
		{
			if (InputPortValue[InPortIndex[portIndex]] & InPortMask[portIndex])
			{
				portStatus->IPStatus[portIndex] = 1;
			}
			else
			{
				portStatus->IPStatus[portIndex] = 0;
			}
		}
		
	}

	for (portIndex = 0; portIndex < MAX_PORT_COUNT; portIndex++)
	{
		if (PortMapInfo.VOPMap[portIndex] == POP_NOT_EXIST)
		{
			portStatus->OPStatus[portIndex] = 0;
		}
		else
		{
			if (OutputPortValue[OutPortIndex[portIndex]] & OutPortMask[portIndex])
			{
				portStatus->OPStatus[portIndex] = 1;
			}
			else
			{
				portStatus->OPStatus[portIndex] = 0;
			}
		}		
	}

	if(THREED_CAMERA_SENSOR & 0x1)
	{
		ThreeDSensorStatus[0] = 1;
	}
	else
	{
		ThreeDSensorStatus[0] = 0;	
	}

	if(THREED_CAMERA_SENSOR & 0x2)
	{
		ThreeDSensorStatus[1] = 1;
	}
	else
	{
		ThreeDSensorStatus[1] = 0;	
	}
}

void PORT_AO_Write(unsigned short aoIndex, unsigned short aoValue)
{
	switch (aoIndex)
	{
		case 0:
			*ANALOG_OUTPUT_0 = aoValue;
			break;
		case 1:
			*ANALOG_OUTPUT_1 = aoValue;
			break;
		case 2:
			*ANALOG_OUTPUT_2 = aoValue;
			break;
		case 3:
			*ANALOG_OUTPUT_3 = aoValue;
			break;
	}
}

void PIO05_PORT_ONOFF()
{
	int i =0;
	
	if((MachineSetupData.CompanyID == DOMESTIC_MACHINE2) || (MachineSetupData.CompanyID == INTERNATIONAL_MACHINE1))
	{
		
		if(portInfo.OPMask&0xFFFF)
		{
			for(i =0 ; i < MAX_PIO5_PORT_NUM ; i++)
			{

				if(portInfo.OPMask & 0x1<<i)
				{
					//ErrorUnit_Put(BoardID, ERROR_TYPE_DEBUG,ERROR_CODE_NONE,portInfo.OPMask,portInfo.OPMask,0000);
					if(MachineSetupData.CompanyID == DOMESTIC_MACHINE2 && i == 1)
					{
						PORT_ChangeOutPort(i+MAX_PIO5_PORT_OFFSET,0);
					}
					else
						PORT_ChangeOutPort(i+MAX_PIO5_PORT_OFFSET,1);

				}
				else
				{
					if(MachineSetupData.CompanyID == DOMESTIC_MACHINE2 && i == 1)
					{
						PORT_ChangeOutPort(i+MAX_PIO5_PORT_OFFSET,1);
					}
					else
						PORT_ChangeOutPort(i+MAX_PIO5_PORT_OFFSET,0);
				}
			}
		}
		else
		{
			for(i =0 ; i < MAX_PIO5_PORT_NUM ; i++)
			{
				PORT_ChangeOutPort(i+MAX_PIO5_PORT_OFFSET,0);
			}
		
		}	
	}
}


