#include <EEPROM.h>
#include "Arduino.h"
#define DEBUG

bool cstate[3]; // 현재 상태(이전 상태와 비교하여 상태변화를 확인한다)
bool pstate[3]; // 이전 상태
bool bchange[3]; //특정핀의 상태가 변경되었는가?
unsigned long clicktime[3]; //각 버튼 down 상태의 밀리초를 저장한다(롱 클릭 시간 확인용)
unsigned long ontime = 0;
unsigned long ledtime = 0;

//bool bCharge = false; //클라이언트 7번
bool bHWError = false; //클라이언트 8번
bool bSelected = false; //현재 선택된 클라이언트 (LED가 깜박인다)
bool bEnbCall =false; //콜 가능여부
bool runtimeledon=false;
String version = "2411291722";
String recvbuffer = "";
uint8_t clientno = 0;
uint8_t offmode = 0;
uint8_t clickcnt = 0;

int pinIn[3] = { 2, 3 , 4 };
int pinOut[3] = { 5, 6 , 7 };


void setup() {

delay(2000);


#ifdef DEBUG
	Serial.begin(19200);
#endif	
Serial1.begin(9600);

 //; Serial.println("Setup Start");
	for (int i = 0; i < 3; i++)
	{
		pinMode(pinIn[i], INPUT_PULLUP);
		pinMode(pinOut[i], OUTPUT);
		digitalWrite(pinOut[i], HIGH); //처음켜지면 ON상태로 한다.
		cstate[i] = false;
		pstate[i] = false;
		clicktime[i] = 0;
	}

 //digitalWrite(13,HIGH);

///  Serial.println("Serial1 Wait");
#ifdef DEBUG
	clicktime[0] = millis();
	while (!Serial) {
		unsigned long runtime = millis() - clicktime[0];
		if (runtime >= 1000) {
      //bSerial=false;
     // Serial.println("Serial1 timeout");
		  break;
		}
		// wait for serial port to connect. Needed for native USB port only
	}
#endif

 //digitalWrite(13,LOW);
	clientno = EEPROM.read(0);  //내부롬의 0번주소에서 클라이언트 번호를 확인한다.
//#ifdef DEBUG	
	Serial.print("AGV Caller v");// 1904020000");
	Serial.println(version);
	Serial.print("ready #");
	Serial.print(clientno);
	Serial.println();
//#endif
	//초기화 완료되면 oFF 한다.
	digitalWrite(pinOut[0], LOW);
	digitalWrite(pinOut[1], LOW);
	digitalWrite(pinOut[2], LOW);
	offmode = 0;

  digitalWrite(13,LOW);
}


void loop() {

 
  #ifdef DEBUG
  unsigned long runtimeled = millis() -ledtime;
  if(runtimeled > 200)
  {
    if(runtimeledon==true)
    {
      digitalWrite(13,LOW);
      runtimeledon=false;
    }else{
      digitalWrite(13,HIGH);
      runtimeledon=true;
    }
    runtimeled= millis();
  }
  #endif
  
	//버튼입력상태를 확인한다.
	for (int i = 0; i < 3; i++)
	{
		cstate[i] = !digitalRead(pinIn[i]);
		if (cstate[i] != pstate[i])
		{
			/*Serial.print("#");
			Serial.print(pinIn[i]);
			Serial.print(" changed");
			Serial.println(cstate[i]);*/

			bchange[i] = true;
			
			if (cstate[i]) //클릭시
			{
				clicktime[i] = millis();
				//Serial.print("SET TIME #");
				//Serial.print(i);
				//Serial.println();
			}
		}
	}

	//버튼 상태변화를 감지
	if (cstate[0] && cstate[1]) //두버튼이 동시에 눌려있다면 디버그모드 진입여부를 확인한다.
	{
		unsigned long time1 = millis() - clicktime[0];
		unsigned long time2 = millis() - clicktime[1];
		if (time1 > 3000 && time2 > 3000)
		{
			if (offmode == 0)
			{
				//OFF 상태일때에는 시리얼 버퍼처리를 하지 않는다.
				offmode = 1;
				#ifdef DEBUG
				Serial.println("ENTER MODE SELECT");
				#endif
			}
		}
		//동시에 눌려있지만 아직 지정 시간전이므로 처리하지 않는다.
	}
	else if (cstate[0] || cstate[1] || cstate[2])	//버튼이 하나만 눌려있을때 
	{
		if (offmode == 2)	//client set mode
		{
			if (cstate[0] && bchange[0])
			{
				clickcnt += 1;
				#ifdef DEBUG
				Serial.print("Click Count = ");
				Serial.println(clickcnt);
				#endif
			}
			else if (cstate[1] && bchange[1])
			{
				//두번째 버튼을 눌렀다면 완료를 의미한다.
				clientno = clickcnt;
				SaveNo(); //현재 값을 저장 해준다.

				//선택된 번호를 깜박임으로 알려준다.
				delay(100);
				digitalWrite(pinOut[0], LOW);
				digitalWrite(pinOut[1], LOW);
				delay(1000);
				for (uint8_t i = 1; i <= clientno; i++)
				{
					digitalWrite(pinOut[0], HIGH);
					delay(500);
					digitalWrite(pinOut[0], LOW);
					delay(500);
				}
				#ifdef DEBUG
				Serial.print("New Client Complete No=");
				Serial.println(clientno);
				#endif
				offmode = 0; //다시 처음상태로 이동
			}

		}
		else if (offmode == 3)	//off mode
		{
			if (bchange[0] || bchange[1])
			{
				Serial1.begin(9600); //re open port
				#ifdef DEBUG
				Serial.println("OFF MODE DISABLE -> NORMAL MODE");
				#endif
				offmode = 0; //일반상태로 전환
			}
		}
		else if (offmode == 1)	//mod select
		{
			if (bchange[0] && cstate[0])	//0번버튼이 ON 되었다면  번호편집모드로 전환
			{
				#ifdef DEBUG
				Serial.println("NO. SET MODE");
				#endif
				clickcnt = 0; //클릭횟수를 초기화
				offmode = 2;
			}
			else if (bchange[1] && cstate[1])	//1번버튼이  ON 되었다면 OFF 모드로 전환
			{
				Serial1.end();
				#ifdef DEBUG
				Serial.println("OFF MODE : Disable Serial for xbee");
				#endif
				offmode = 3;
			}
		}
		else if (offmode == 0) {	//일반 모드일때 하나만 눌렸다면 해당 버튼 명령을 전송한다.

			//uint8_t clientascii = (uint8_t)(clientno + 30);
	
			if (bchange[0] && cstate[0])
			{
        		
				#ifndef DEBUG
			digitalWrite(13,HIGH);
			delay(200);
			digitalWrite(13,LOW);
			#endif

        Serial1.clearWriteError();
				Serial1.write(0x02);
				Serial1.print(clientno);
				Serial1.print('1'); //call              				
				Serial1.write(0x03);				
				Serial1.flush();
     
        #ifdef DEBUG
				Serial.print("SEND CALL #");
				Serial.println(clientno);
				#endif
			}
			else if (bchange[1] && cstate[1])
			{		
				
				#ifndef DEBUG
			digitalWrite(13,HIGH);
			delay(200);
			digitalWrite(13,LOW);
			#endif
        
        Serial1.clearWriteError();
				Serial1.write(0x02);
				Serial1.print(clientno);
				Serial1.print('0'); //call              				
				Serial1.write(0x03);
				Serial1.flush();
#ifdef DEBUG
				Serial.print("SEND CANCEL#");
				Serial.println(clientno);
				#endif
			}
			else if (bchange[2] && cstate[2]) //QA진행 버튼이 눌렸을때
			{
					
						#ifndef DEBUG
			digitalWrite(13,HIGH);
			delay(200);
			digitalWrite(13,LOW);
			#endif

        Serial1.clearWriteError();
				Serial1.write(0x02);
				Serial1.print(clientno);
				Serial1.print('2'); //go qa
				Serial1.write(0x03);
				Serial1.flush();
#ifdef DEBUG
				Serial.print("SEND QA#");
				Serial.println(clientno);
				#endif
			}
		}
	}

	//Serial.print("mode=");
	//Serial.println(offmode);
	//LED상태 조정
	switch (offmode)
	{
	case 0:	//일반 상태값(버튼을 누르면 해당 버튼의 LED를 조정한다)
		digitalWrite(pinOut[0], cstate[0]);
		digitalWrite(pinOut[1], cstate[1]);
		digitalWrite(pinOut[2], cstate[2]);
   //Serial.print("led 3 stat=");
   //Serial.println(cstate[2]);
		break;
	case 1: //모드 선택 상태( 이때 OK를 누르면 번호셋, CANCEL을 누르면 OFF 상태로)
		if (digitalRead(pinOut[0]) == true)
		{
			digitalWrite(pinOut[1], HIGH);
			digitalWrite(pinOut[0], LOW);
		}
		else {
			digitalWrite(pinOut[0], HIGH);
			digitalWrite(pinOut[1], LOW);
		}
		delay(100);
		break;
	case 2: //클라이언트 번호 설정 모드 - 적색만 깜박이고 녹색의 클릭횟수로 번호를 설정한다.
		if (digitalRead(pinOut[1]) == true)
			digitalWrite(pinOut[1], LOW);
		else
			digitalWrite(pinOut[1], HIGH);
		break;
		delay(100);
	case 3: //OFF 상태일때에는 LED를 서로 ON/OFF 한다.
		if (digitalRead(pinOut[0]) == true)
		{
			digitalWrite(pinOut[1], LOW);
			digitalWrite(pinOut[0], LOW);
		}
		else {
			digitalWrite(pinOut[0], HIGH);
			digitalWrite(pinOut[1], HIGH);
		}
		delay(100);
	}


	//상태변환 여부를 초기화 
	if (bchange[0]) pstate[0] = cstate[0];
	if (bchange[1]) pstate[1] = cstate[1];
	if (bchange[2]) pstate[2] = cstate[2];
	bchange[0] = false;
	bchange[1] = false;
	bchange[2] = false;
	
	//일반모드일때 처리 내역
	if (offmode == 0)
	{
		unsigned long runtime = millis() - ontime;

    //CALL가능하면 3번 LED를 ON 한다 230119
    if(bEnbCall)
    {
      digitalWrite(pinOut[2],HIGH);
    } else{
      if(cstate[2])
      digitalWrite(pinOut[2],HIGH);
      else
      digitalWrite(pinOut[2],LOW);
    }

		//현재 개체가 AGV선택개체라면 LED를 깜박거려서 표시를 해준다.(녹색버튼기준)    
		if (bHWError)
		{
			//하드웨어 오류 발생시에는 적색을 깜박거린다.(녹색:OFF)
			digitalWrite(pinOut[0], LOW); //녹색은 끈다.

			//적색을 깜박인다.(1초주기로 ON/OFF 한다)
			bool state = digitalRead(pinOut[1]);
			if (runtime >= 250)	//190319
			{
				if (state)  digitalWrite(pinOut[1], LOW);
				else		digitalWrite(pinOut[1], HIGH);
				ontime = millis();
			}
		}
		else if (bSelected)
		{
			digitalWrite(pinOut[1], HIGH); //적색은 일단 켜둔다.

			//선택된 개체일때에는 적색은 켜고 녹색은 끔박인다.
			bool state = digitalRead(pinOut[0]);
			
			//500ms 단위로 깜박인다.
			if (runtime >= 500)	//190319
			{
				if (state)  digitalWrite(pinOut[0], LOW);
				else digitalWrite(pinOut[0], HIGH);
				ontime = millis();
			}
		}
		else {
			//선택개체가 아니라면 LED를 모두 OFF해준다.
			for (int i = 0; i < 2; i++)
			{
				if (digitalRead(pinIn[0]) != digitalRead(pinOut[0]))
					cstate[i] = !cstate[i];    //이전상태값을 반전시켜줌으로 써 다음 루프때 LED가 꺼지게 한다.
			}
		}
	}

	//외부디버그 명령을 처리한다.
	#ifdef DEBUG
	CheckRemoteDebugCommand();
	#endif
	CheckReceiveXbee();
	delay(100);
}

void CheckRemoteDebugCommand()
{
	if (Serial.available() > 0)
	{
		int recv = Serial.read();
		if (recv == 0x0A || recv == 0x0D)
		{
			RunCommand();
			recvbuffer = "";
		}
		else {
			recvbuffer += (char)recv;
		}
	}
}

String buffer = "";
int kitNo = 0;
int incomingByte = 0;
void CheckReceiveXbee()
{
	//수신된 자료가 잇는지 체크한다.
#ifndef DEBUG
    digitalWrite(13,HIGH);
#endif

	while (Serial1.available() > 0) {
		incomingByte = Serial1.read();

#ifdef DEBUG
		Serial.print("[RX]");
		Serial.println(incomingByte, HEX);

#endif

		//stx ,etx 는 hex 처리하고 번호와 값을 ascii처리함
		if (incomingByte == 0x02)
		{
			buffer = "";
		}
		else if (incomingByte == 0x03) {

			//ETX값이나 전체 길이가 4가아니라면 추가해야함
			byte clientno = (byte)(buffer.substring(0, 1).toInt());
			char clientval = buffer[1];// .substring(1, 2).toInt());
      char enablecall = '0';  //콜가능여부 추가 230119

      //데이터가 3바이트라면 콜 가능상태로 전송된 경우이다 230119
      if(buffer.length() > 2)
      {
        enablecall = buffer[2];
      }
        
			NewMsgEvent(clientno, clientval,enablecall);
			break;
		}
		else{
			buffer += (char)incomingByte;	//문자를 누적시킨다.
		}
	}

  #ifndef DEBUG
  digitalWrite(13,LOW);
  #endif
}

/* 190319 */
void NewMsgEvent(uint8_t no, char value,char canCall)
{
	if (no == 1) return;	//충전기관련 코드는 처리 하지 않는다. 190417
	else if (no == 8)	//AGV가 HW에러 발생시 이 값을 전송한다.
	{
		if (bHWError == false) bHWError = true;
		#ifdef DEBUG
		if (value == '1')//system error
		{
			Serial.println("FLAG ON : H/W ERROR");
		}
		else {
			Serial.println("FLAG ON : AGV MANUAL MODE");
		}
		#endif
		return;
	}

	//그외 나머지 상태에는 오류사항을 해제 해준다.
	if (bHWError == true)
	{
		bHWError = false;
		#ifdef DEBUG
		Serial.println("FLAG OFF : H/W ERROR OFF");
		#endif
	}

  bEnbCall = (canCall == '1');

	//누군가가 지정되어있다.
	if (value == '1')
	{
		if (no != clientno)	//해당 번호가 내 번호와 일치하지 않는다면 내가 지정된 경우가 아니다.
		{
			//서버로부터 어떠한 개체가 켜졌는데. 그게 내가 아니라면 나는 OFF 한다.
			if (bSelected) bSelected = false;
			#ifdef DEBUG
			Serial.print("Selected Off by Another ON No=");
			Serial.println(no);
			#endif
		}
		else {
			bSelected = true; //내 번호가 켜졌으므로 선택됨으로 한다.
			#ifdef DEBUG
			Serial.print("Selected On No=");
			Serial.println(no);
			#endif
		}
	}
	else if (value == '0')	//누군가가 OFF 되어있다.
	{
		if (no == clientno)
		{
			//내가 꺼졋음을 보내왔다.
			bSelected = false;
			#ifdef DEBUG
			Serial.println("Selected Off");
			#endif
		}
	}
}

void RunCommand()
{

	Serial.print("Remote Control : ");
	Serial.println(recvbuffer);

	if (recvbuffer == "on")
	{
		Serial.print("on clientno = ");
		Serial.println(clientno);
		NewMsgEvent(clientno, '1','1');
	}
 else if (recvbuffer == "callon")
 {
    Serial.print("on clientno = ");
    Serial.println(clientno);
    NewMsgEvent(clientno, '0','1');
  }
  else if (recvbuffer == "calloff")
 {
    Serial.print("on clientno = ");
    Serial.println(clientno);
    NewMsgEvent(clientno, '0','0');
  }
	else if (recvbuffer == "off") {
		Serial.print("off clientno = ");
	Serial.println(clientno);
		NewMsgEvent(clientno, '0','0');
	}
	else if (recvbuffer == "hw") {
		
		bHWError = !bHWError;
		Serial.print("bHWError=");
		Serial.println(bHWError);
	}
	else if (recvbuffer == "set") {
		Serial1.print("+++");
		Serial1.write(0x0D);
		Serial1.flush();
	}
	else if (recvbuffer == "h")
	{
		Serial.println("AGV Caller");
		Serial.print("v");
		Serial.println(version);
		Serial.println("==============");
		Serial.println("h : help");
		Serial.println("on : selected on");
		Serial.println("off : selected off");
		Serial.println("#1~6 : set client no");
		Serial.println("? : get client no");
		Serial.println("==============");
	}
	else if (recvbuffer == "?")
	{
		Serial.print("Get Client No : #");
		Serial.println(clientno);
	}
	else if (recvbuffer.startsWith("#"))
	{
		String strNo = recvbuffer.substring(1);
		clientno = (byte)(strNo.toInt());
		SaveNo(); // EEPROM.write(0, clientno);
	}
}
void SaveNo()
{
	EEPROM.write(0, clientno);
	#ifdef DEBUG
	Serial.print("Save Client No : #");
	Serial.println(clientno);
	#endif
}