#include "IO.h"
#include "VarClass.h"
#include "motor.h"
#include "HmiClass.h"
#include "UtilClass.h"

void IoClass::Setup()
{
	hmi.SendMessage("##:IO:SETUP", false);

	byte DIPins[] = { 30,31,32,33,34,35,36,37,38,39,40,41,42,43,44,45 };
	byte DOPins[] = { 22,23,24,25,26,27,28,29 };

	//입력포트 설정
	for (int i = 0; i < sizeof(DIPins); i++)
		pinMode(DIPins[i], INPUT);

	//출력포트설정
	for (int i = 0; i < sizeof(DOPins); i++)
		pinMode(DOPins[i], OUTPUT);

	//MAF 초기화
	for (int i = 0; i < MAF_SIZE; i++)
	{
		bufferF[i] = 0;
		bufferB[i] = 0;
	}

}
void IoClass::Update()
{

	//Read DigitalInput

	bool diValue[16]; int idx = 0;
	diValue[idx++] = digitalRead(PINI_EMG);	//B접이다
	diValue[idx++] = digitalRead(PINI_BTN_1);
	diValue[idx++] = digitalRead(PINI_BTN_2);
	diValue[idx++] = digitalRead(PINI_BTN_3);
	diValue[idx++] = digitalRead(PINI_BTN_4);
	diValue[idx++] = digitalRead(PINI_OVERLOADL1);
	diValue[idx++] = digitalRead(PINI_OVERLOADL2);
	diValue[idx++] = digitalRead(PINI_OVERLOADR1);
	diValue[idx++] = digitalRead(PINI_OVERLOADR2);
	diValue[idx++] = digitalRead(PINI_EMPTY_38);	
	diValue[idx++] = digitalRead(PINI_BTN_ZUP);
	diValue[idx++] = digitalRead(PINI_BTN_ZDN);
	diValue[idx++] = digitalRead(PINI_LIMIT_LU);
	diValue[idx++] = digitalRead(PINI_LIMIT_LD);
	diValue[idx++] = digitalRead(PINI_LIMIT_RU);
	diValue[idx++] = digitalRead(PINI_LIMIT_RD);
	diValue[idx++] = digitalRead(PINI_STOP);


  //비상정지 반전
  diValue[IDXI_EMG] = !diValue[IDXI_EMG];

  //오버로드센서 반전
  diValue[IDXI_OVERLOADL] = !diValue[IDXI_OVERLOADL];
  diValue[IDXI_OVERLOADR] = !diValue[IDXI_OVERLOADR];
  diValue[IDXI_EMPTY7] = !diValue[IDXI_EMPTY7];
  diValue[IDXI_EMPTY8] = !diValue[IDXI_EMPTY8]; 
  
  //오버로드센서를 합친다  5+6=>5, 7+8=>6
  diValue[IDXI_OVERLOADL] = diValue[IDXI_OVERLOADL] || diValue[IDXI_OVERLOADR];
  diValue[IDXI_OVERLOADR] = diValue[IDXI_EMPTY7] || diValue[IDXI_EMPTY8];
  diValue[IDXI_EMPTY7]= false;
  diValue[IDXI_EMPTY8]= false;
  
  //리밋센서도 반전
  diValue[IDXI_LIMIT_LU] = !diValue[IDXI_LIMIT_LU];
  diValue[IDXI_LIMIT_LD] = !diValue[IDXI_LIMIT_LD];
  diValue[IDXI_LIMIT_RU] = !diValue[IDXI_LIMIT_RU];
  diValue[IDXI_LIMIT_RD] = !diValue[IDXI_LIMIT_RD]; 
	
	//리버스 체크 200325
  //입력값은 Reverse 하지 않는다
  /*
  var._eep_pindir_iL = 0;
  var._eep_pindir_iH = 0;

	for (int i = 0; i < 16; i++)
	{
		if (i < 8)
		{
			if (bitRead(var._eep_pindir_iL, i) == true)
				diValue[i] = !diValue[i];
		}
		else {
			if (bitRead(var._eep_pindir_iH, i - 8) == true)
				diValue[i] = !diValue[i];
		}
	}*/


	for (int i = 0; i < idx; i++)
	{
		//이 값의 변화를 체크한다.
		bool oldValue = bitRead(var.IOData, i);
		bool isNewValue = false;
		if (oldValue != diValue[i]) isNewValue = true;
		bitWrite(var.IOData, i, diValue[i]);//0번부터 기록한다

		//값이 변화했따
		if (isNewValue) IOValueChanged(false, i, diValue[i]);
	}

	//read output
	bool doValue[8]; idx = 0;
	doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_LDIR);
	doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_LRUN);
	doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_RDIR);
	doValue[idx++] = digitalRead(PINO_GUIDEMOTOR_RRUN);
	doValue[idx++] = digitalRead(PINO_EMPTY_26);
	doValue[idx++] = digitalRead(PINO_EMPTY_27);
	doValue[idx++] = digitalRead(PINO_EMPTY_28);
	doValue[idx++] = digitalRead(PINO_EMPTY_29);

	for (int i = 0; i < idx; i++)
	{
		bool oldValue = bitRead(var.IOData, i + 16);
		bool isNewValue = false;
		if (oldValue != doValue[i]) isNewValue = true;
		bitWrite(var.IOData, i + 16, doValue[i]);//0번부터 기록한다

		//값이 변화했따
		if (isNewValue) IOValueChanged(true, i, !oldValue);
	}

	//Read Analog (마그넷 센서 값만 확인한다)
	idx = 0;

	//원본값은 그대로 저장한다
	var.ANData[idx++] = analogRead(PINAI_0);
	var.ANData[idx++] = analogRead(PINAI_1);
	var.ANData[idx++] = analogRead(PINAI_2);
	var.ANData[idx++] = analogRead(PINAI_3);

	idx = 0;
	var.AOData[idx++] = 0;
	var.AOData[idx++] = 0;
	var.AOData[idx++] = 0;
	var.AOData[idx++] = 0;

	//파워체크
	if (IsPowerLoss() == true) var.setFlag(FLAG_POWERLOSS, true, "IOUpdate");
	else  var.setFlag(FLAG_POWERLOSS, false, "IOUpdate");
}

//메인전원이 OFF되어있는지 확인합니다.(리밋센서의OFF여부로 체크함)
bool IoClass::IsPowerLoss()
{
	//얘들은 B접점이라서. 항상 ON 이 되어있는 애들이다.
	//전원이 OFF되면 모든 센서가 LOW 상태가 된다
	//하지만 신호를 반전(!) 시켜두었으므로 모두 켜진다면 OFF 된 경우이다)
	bool b1 = bitRead(var.IOData, IDXI_LIMIT_LD);
	bool b2 = bitRead(var.IOData, IDXI_LIMIT_LU);
	bool b3 = bitRead(var.IOData, IDXI_LIMIT_RU);
	bool b4 = bitRead(var.IOData, IDXI_LIMIT_RD);
	if (b1 == true && b2 == true && b3 == true && b4 == true) return true;
	return false;
}

//IO상태값이 변화되었을때 동작한다
void IoClass::IOValueChanged(bool isout, uint8_t index, bool newValue)
{
	if (isout == false) //입력포트
	{

    if (index == IDXI_BTN_1)
    {
      if (newValue == true)
      {
          hmi.SendMessage("button 1 pressed", false);
      }
    }
    if (index == IDXI_BTN_2)
    {
      if (newValue == true)
      {
          hmi.SendMessage("button 2 pressed", false);
      }
    }
    if (index == IDXI_BTN_3)
    {
      if (newValue == true)
      {
          hmi.SendMessage("button 3 pressed", false);
      }
    }
    if (index == IDXI_BTN_4)
    {
      if (newValue == true)
      {
          hmi.SendMessage("button 4 pressed", false);
      }
    }
    
		//양쪽면의 z축 UP/DN 버튼 할당
		if (index == IDXI_BTN_ZUP)
		{
			if (newValue == true)
			{
				if (mot.GetZDirL() == ZDIR_CW && mot.IsMoveZ() == true)
				{
					hmi.SendMessage("z-stop by h/w button", false);
					mot.SetZRun(ZRUN_STOP);
				}
				if (mot.GetZDirL() == ZDIR_CCW && mot.IsMoveZ() == true)
				{
					hmi.SendMessage("z-stop by h/w button", false);
					mot.SetZRun(ZRUN_STOP);
				}
				else {
					mot.SetZRun(ZRUN_UP);
					hmi.SendMessage("Z-Up by H/W Button", false);
				}

			}
		}
		else if (index == IDXI_BTN_ZDN)
		{
			if (newValue == true)
			{
				if (mot.GetZDirL() == ZDIR_CCW && mot.IsMoveZ() == true)
				{
					hmi.SendMessage("z-stop by h/w button", false);
					mot.SetZRun(ZRUN_STOP);
				}
				if (mot.GetZDirL() == ZDIR_CW && mot.IsMoveZ() == true)
				{
					hmi.SendMessage("z-stop by h/w button", false);
					mot.SetZRun(ZRUN_STOP);
				}
				else {
					mot.SetZRun(ZRUN_DN);
					hmi.SendMessage("Z-Dn by H/W Button", false);
				}
			}
		}
		else if (index == IDXI_EMG)	//비상정지 혹은 범퍼센서가 감지된경우
		{
			if (newValue == true)
			{
				hmi.SendMessage("EMG Stop Detected", false);
				mot.SetZRun(ZRUN_STOP);
			}
		}

		//일반적으로 Rising 상태일때에만 동작시킨다.		
		if (newValue == true)
		{
			//하단센서2개가 들어왔다면 FLAG를 설정한다
			if (index == IDXI_LIMIT_LD || index == IDXI_LIMIT_RD)
			{
				if (index == IDXI_LIMIT_LD )
				{
					if (var.getFlag(FLAG_LIMITLOWL) == false)
					{
						var.setFlag(FLAG_LIMITLOWL, true, "IO:Limit Sensor(LD) Changed");
						hmi.SendMessage(F("SET:FLAG:LOW_LIMITL:1"), false);
					}
				}
				else if (index == IDXI_LIMIT_RD )
				{
					if (var.getFlag(FLAG_LIMITLOWR) == false)
					{
						var.setFlag(FLAG_LIMITLOWR, true, "IO:Limit Sensor(RD) Changed");
						hmi.SendMessage(F("SET:FLAG:LOW_LIMITR:1"), false);
					}
				}
				else {
					bool  b1 = bitRead(var.IOData, IDXI_LIMIT_LD);
					bool  b2 = bitRead(var.IOData, IDXI_LIMIT_RD);
					if (b1 == false && var.getFlag(FLAG_LIMITLOWL) == true)
					{
						var.setFlag(FLAG_LIMITLOWL, false, "IO:Limit Sensor(LD) Changed");
						hmi.SendMessage(F("SET:FLAG:LOW_LIMITL:0"), false);
					}
					if (b2 == false && var.getFlag(FLAG_LIMITLOWR) == true)
					{
						var.setFlag(FLAG_LIMITLOWR, false, "IO:Limit Sensor(RD) Changed");
						hmi.SendMessage(F("SET:FLAG:LOW_LIMITR:0"), false);
					}
				}
			}
						
			if (index == IDXI_LIMIT_LU || index == IDXI_LIMIT_RU)
			{
				

				if (index == IDXI_LIMIT_LU || var.getFlag(FLAG_LIMITHIGHL) == false)
				{
					var.setFlag(FLAG_LIMITHIGHL, true, "IO:Limit Sensor(LU) Changed");
					hmi.SendMessage(F("SET:FLAG:HIGH_LIMITL:1"), false);
				}
				else if (index == IDXI_LIMIT_RU || var.getFlag(FLAG_LIMITHIGHR) == false)
				{
					var.setFlag(FLAG_LIMITHIGHR, true, "IO:Limit Sensor(RU) Changed");
					hmi.SendMessage(F("SET:FLAG:HIGH_LIMITR:1"), false);
				}
				else {
					bool  b1 = bitRead(var.IOData, IDXI_LIMIT_LU);
					bool  b2 = bitRead(var.IOData, IDXI_LIMIT_RU);
					if (b1 == false && var.getFlag(FLAG_LIMITHIGHL) == true)
					{
						var.setFlag(FLAG_LIMITHIGHL, false, "IO:Limit Sensor(LU) Changed");
						hmi.SendMessage(F("SET:FLAG:HIGH_LIMITL:0"), false);
					}
					if (b2 == false && var.getFlag(FLAG_LIMITHIGHR) == true)
					{
						var.setFlag(FLAG_LIMITHIGHR, false, "IO:Limit Sensor(RU) Changed");
						hmi.SendMessage(F("SET:FLAG:HIGH_LIMITR:0"), false);
					}
				}
				
			}
			
			//오버로드 플래그 활성화
			if (index == IDXI_OVERLOADL)
			{
				mot.SetZRun(ZRUN_STOP);
				hmi.SendMessage(F("All Mot Stop by OverloadL"), false);
			}
			else if (index == IDXI_OVERLOADR)
			{
				mot.SetZRun(ZRUN_STOP);
				hmi.SendMessage(F("All Mot Stop by OverloadR"), false);
			}

		}
		else  	//해당값이 OFF 되었다
		{

			//대상이 리밋센서라면 플래그를 설정 해준다
			if (index == IDXI_LIMIT_LD)
			{

				hmi.SendMessage("Limit Sensor(LD) Change " + String(index) + "=" + String(newValue), false);
				if (var.getFlag(FLAG_LIMITLOWL) == true)
				{
					var.setFlag(FLAG_LIMITLOWL, false, "IO:Limit Sensor Changed(LD)");
					hmi.SendMessage(F("SET:FLAG:LOW_LIMIT:0"), false);
				}
			}
			else if (index == IDXI_LIMIT_RD)
			{

				hmi.SendMessage("Limit Sensor(RD) Change " + String(index) + "=" + String(newValue), false);
				if (var.getFlag(FLAG_LIMITLOWR) == true)
				{
					var.setFlag(FLAG_LIMITLOWR, false, "IO:Limit Sensor Changed(RD)");
					hmi.SendMessage(F("SET:FLAG:LOW_LIMIT:0"), false);
				}
			}
			else if (index == IDXI_LIMIT_LU )
			{
				hmi.SendMessage("Limit Sensor(LU) Change " + String(index) + "=" + String(newValue), false);
				if (var.getFlag(FLAG_LIMITHIGHL) == true)
				{
					var.setFlag(FLAG_LIMITHIGHL, false, "IO:Limit Sensor Changed(LU)");
					hmi.SendMessage(F("SET:FLAG:HIGH_LIMIT:0"), false);
				}
			}
			else if (index == IDXI_LIMIT_RU)
			{
				hmi.SendMessage("Limit Sensor(RU) Change " + String(index) + "=" + String(newValue), false);
				if (var.getFlag(FLAG_LIMITHIGHR) == true)
				{
					var.setFlag(FLAG_LIMITHIGHR, false, "IO:Limit Sensor Changed(RU)");
					hmi.SendMessage(F("SET:FLAG:HIGH_LIMIT:0"), false);
				}
			}
		}
	}
	else //출력포트
	{
		//일반적으로 Rising 상태일때에만 동작시킨다.		
		if (newValue == true)
		{

		}
	}
}

IoClass io;