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

void motor::Setup()
{
	hmi.SendMessage("##:MOT:SETUP", false);

	//Z축 멈춤
	SetZRun(ZRUN_STOP);
}
void motor::Update()
{
	//##########################
	//      사용자추가코드
	//##########################

	if (var.pingtime == 0 || var.pingtime > millis()) var.pingtime = millis();
	long runtime = millis() - var.pingtime;

	//중지조건 체크
	//(충전중, 비상정지, 오버로드, 설정화면)
	bool bStop1 = var.getFlag(FLAG_SETUP);
	bool bStop2L = bitRead(var.IOData, IDXI_OVERLOADL);
	bool bStop2R = bitRead(var.IOData, IDXI_OVERLOADR);
	bool bStop3 = bitRead(var.IOData, IDXI_EMG);
	bool bStop4 = bitRead(var.IOData, IDXI_STOP);

	//중지조건이 활성화되었다면 처리
	String stpReason = "";
	
	//Z축 중지는 오버로드만 확읺ㄴ다.
	if (bStop3== true)
	{
		if (var.getFlag(FLAG_STOPZ) == false)
		{
			dbgSerial.println("Set Flag On: flag_stopz by EMG");
			var.setFlag(FLAG_STOPZ, true, "MotUpdate");
		}
	}
	else if (bStop2L == true)
	{
		if (var.getFlag(FLAG_STOPZ) == false)
		{
			dbgSerial.println("Set Flag On: flag_stopz by OverloadL");
			var.setFlag(FLAG_STOPZ, true, "MotUpdate_OVLL");
		}
	}
	else if (bStop2R == true)
	{
		if (var.getFlag(FLAG_STOPZ) == false)
		{
			dbgSerial.println("Set Flag On: flag_stopz by OverloadR");
			var.setFlag(FLAG_STOPZ, true, "MotUpdate_OVLR");
		}
	}
	else if (var.getFlag(FLAG_STOPZ) == true)
	{
		dbgSerial.println("Set Flag Off");
		var.setFlag(FLAG_STOPZ, false, "MotUpdate");
	}

	//가이드 모터 동작시 Limit 센서 체크
	ZLimit_AutoStop();

	if (var.getFlag(FLAG_STOPZ) == true)
	{
		if (IsMoveZ() == true)
		{
			Serial.println("Z-Axis Auto Stop by 'STOP-Z' Condition");
			mot.SetZRun(ZRUN_STOP);
		}
	}

	//Serial.println("motor update");
}


//Z축이 동작죽이라면 리밋센서값을 보고 자동으로 멈춥니다
void motor::ZLimit_AutoStop()
{
	if (mot.IsMoveZ() == true)
	{
		//정방향이동시(올라간다)
		if (mot.GetZDirL() == ZDIR_CW)
		{
			if (bitRead(var.IOData, IDXI_OVERLOADL) == true )	//오버로드 활성화시 멈춤
			{
				mot.SetZRun(ZRUN_STOP);
				if (var.getFlag(FLAG_SETUP) == false)	hmi.SendMessage("Z-Axis Auto Stop by OverloadL", false);
			}
			else if (bitRead(var.IOData, IDXI_OVERLOADR) == true )	//오버로드 활성화시 멈춤
			{
				mot.SetZRun(ZRUN_STOP);
				if (var.getFlag(FLAG_SETUP) == false)	hmi.SendMessage("Z-Axis Auto Stop by OverloadR", false);
			}
			else {
				//상단센서가 하나라도 동작하면 멈춘다
				if (mot.IsMoveZL() == true && bitRead(var.IOData, IDXI_LIMIT_LU) == true)
				{
					mot.SetZRunL(false);
					if (var.getFlag(FLAG_SETUP) == false)	hmi.SendMessage("Z-Axis (Left) Auto Stop by Upper Limit", false);
				}
				if (mot.IsMoveZR() == true && bitRead(var.IOData, IDXI_LIMIT_RU) == true)
				{
					mot.SetZRunR(false);
					if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis (Right) Auto Stop by Upper Limit", false);
				}
			}
		}
		else  //역방향이동(내려간다)
		{
			//하단센서가 하나라도 동작하면 멈춘다
			if (mot.IsMoveZL() == true && bitRead(var.IOData, IDXI_LIMIT_LD) == true)
			{
				mot.SetZRunL(false);
				if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis (Left) Auto Stop by Lower Limit", false);
			}
			if (mot.IsMoveZR() == true && bitRead(var.IOData, IDXI_LIMIT_RD) == true)
			{
				mot.SetZRunR(false);
				if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis (Right) Auto Stop by Lower Limit", false);
			}
		}
	}
}



//가이드 모터 진행방향을 반환(CW:Up, CCW:Down)
ZDirection motor::GetZDirL()
{
	bool value = bitRead(var.IOData, IDXO_GUIDEMOTOR_LDIR + 16);
	if (value == false) return ZDIR_CW;
	else return ZDIR_CCW;
}
ZDirection motor::GetZDirR()
{
  bool value = bitRead(var.IOData, IDXO_GUIDEMOTOR_RDIR + 16);
  if (value == false) return ZDIR_CW;
  else return ZDIR_CCW;
}

//가이드 모터(좌+우) 동작 여부
bool motor::IsMoveZ()
{
	if (IsMoveZL() == true) return true;
	else if (IsMoveZR() == true) return true;
	else return false;
}

//가이드 모터(좌) 동작 여부
bool motor::IsMoveZL()
{
	bool v1 = bitRead(var.IOData, IDXO_GUIDEMOTOR_LRUN + 16);
	//bool v2 = bitRead(var.IOData, IDXO_GUIDEMOTOR_LEFT + 16);
	return v1 ;
}

//가이드 모터(우) 동작 여부
bool motor::IsMoveZR()
{
	bool v1 = bitRead(var.IOData, IDXO_GUIDEMOTOR_RRUN + 16);
	//bool v2 = bitRead(var.IOData, IDXO_GUIDEMOTOR_RIGHT + 16);
	return v1 ;
}

//가이드모터 진행방향 결정(True:정방향, False:역방향)
void motor::SetZDir(ZDirection direction)
{
	if (direction == ZDIR_CW) {
		digitalWrite(PINO_GUIDEMOTOR_LDIR, LOW);
		digitalWrite(PINO_GUIDEMOTOR_RDIR, LOW);
	}
	else {
		digitalWrite(PINO_GUIDEMOTOR_LDIR, HIGH);
		digitalWrite(PINO_GUIDEMOTOR_RDIR, HIGH);
	}
}

void motor::SetZRun(ZRunMode runMode) {

	bool bEmg = var.getFlag(FLAG_STOPZ);  //Z축이 움직이면 안되는 조건이므로 처리하지 않는다.

	if (runMode == ZRUN_UP)
	{
		if (bEmg) {
			hmi.SendMessage("[Ignore] Z-Run by Stop-Z Flag", true);
			return;
		}
		SetZDir(ZDIR_CW);
		SetZRunL(true);
		SetZRunR(true);
		if (var.getFlag(FLAG_SETUP) == false)	hmi.SendMessage("Guide Up", false);
	}
	else if (runMode == ZRUN_DN)
	{
		if (bEmg) {
			hmi.SendMessage("[Ignore] Run by Stop-Z Flag", true);
			return;
		}
		SetZDir(ZDIR_CCW);
		SetZRunL(true);
		SetZRunR(true);
		if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Guide Dn", false);
	}
	else if (runMode == ZRUN_STOP)
	{
		SetZRunL(false);
		SetZRunR(false);
		if (var.getFlag(FLAG_SETUP) == false)	hmi.SendMessage("Guid Stop", false);
		//digitalWrite(PINO_GUIDEMOTOR_RUN, LOW);
	}
}

void motor::SetZRunL(bool run) {
	if (run == true)
	{
		//if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Left Run", false);
		digitalWrite(PINO_GUIDEMOTOR_LRUN, HIGH);
		//digitalWrite(PINO_GUIDEMOTOR_RUN, HIGH);
	}
	else {
		digitalWrite(PINO_GUIDEMOTOR_LRUN, LOW);
		//L,R둘다 꺼져있다면 STA도 끈다
		//if (bitRead(var.IOData, IDXO_GUIDEMOTOR_RIGHT + 16) == false)
		//{
		//	if (var.getFlag(FLAG_SETUP) == false)hmi.SendMessage("Z-Axis Sta Off by Right", false);
		//	digitalWrite(PINO_GUIDEMOTOR_RUN, LOW);
		//}
	}

}

void motor::SetZRunR(bool run) {
	if (run == true)
	{
		//if (var.getFlag(FLAG_SETUP) == false) hmi.SendMessage("Z-Axis Right Run", false);
		digitalWrite(PINO_GUIDEMOTOR_RRUN, HIGH);
		//digitalWrite(PINO_GUIDEMOTOR_RUN, HIGH);
	}
	else {
		digitalWrite(PINO_GUIDEMOTOR_RRUN, LOW);
		//L,R둘다 꺼져있다면 STA도 끈다
		//if (bitRead(var.IOData, IDXO_GUIDEMOTOR_LEFT + 16) == false)
		//{
		//	if (var.getFlag(FLAG_SETUP) == false)	hmi.SendMessage("Z-Axis Sta Off by Right", false);
		//	digitalWrite(PINO_GUIDEMOTOR_RUN, LOW);
		//}
	}
}


motor mot;