#include "ZigBeeOperator.h"
#include "CommandEncoder.h"
#include "CommandDecoder.h"
#include "AccurateMotor.h"
#include <Ultrasonic.h>
#include <BKRC_Voice.h>
#include "GlobalDatas.h"
#include <CoreBeep.h>
#include <DCMotor.h>
#include <CoreKEY.h>
#include <CoreLED.h>
#include "Handler.h"
#include <BH1750.h>
#include <BEEP.h>

void setup()
{
	Serial.begin(115200);
	CoreLED.Initialization();
	CoreKEY.Initialization();
	CoreBeep.Initialization();
	ZigBeeOperator.Initialization();
	LED.Initialization();
	BH1750.Initialization();
	BEEP.Initialization();
	Infrare.Initialization();
	Ultrasonic.Initialization();
	DCMotor.Initialization(30000);
	BKRC_Voice.Initialization();

	while (!Serial);
	sendflag = 0;
	frisrtime = 0;
	Tcount = 0;

	OpenMVOpt::Servo_Control(0);
}

void loop()
{
	uint8_t si = 0;
	frisrtime = millis();

	CoreKEY.Kwhile(Handler::KEY_Handler);                                  //按键检测

	if (ExtSRAMInterface.ExMem_Read(ZigBee_Read_Address) != 0x00)              //从车接收ZigBee数据
	{
		ExtSRAMInterface.ExMem_Read_Bytes(ZigBee_command, 8);
		CommandDecoder decoder(ZigBee_command);
		if(decoder.CommandValidate())
			Handler::ZigBeeRx_Handler(ZigBee_command);                      //ZigBee接收数据处理              
	}

	if (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //检测OpenMV识别结果
	{
		Data_Type = ExtSRAMInterface.ExMem_Read(0x603A);
		Data_Flag = ExtSRAMInterface.ExMem_Read(0x603B);
		Data_Length = ExtSRAMInterface.ExMem_Read(0x603C);
		Data_Length = Data_Length + 6;
		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OTABuf, Data_Length);
		if ((Data_OTABuf[0] == 0x55) && (Data_OTABuf[1] == 0x02))
		{
			ExtSRAMInterface.ExMem_Write_Bytes(0x6180, Data_OTABuf, Data_Length);  //使用自定义数据区上传OpenMV识别结果
			Handler::OpenMVRx_Handler(Data_OTABuf);                                        //接收OpenMV，数据处理函数
		}
	}

	if (((millis() - frisrtime >= TSendCycle) || (Tcount >= TSendCycle)) && (sendflag == 1))  //获取、上传任务版数据
	{
		uint16_t tp = (uint16_t)(Ultrasonic.Ranging(CM) * 10.0);
		ZigBee_back[5] = (tp >> 8) & 0xff;
		ZigBee_back[4] = tp & 0xff;

		tp = BH1750.ReadLightLevel();
		ZigBee_back[7] = (tp >> 8) & 0xff;
		ZigBee_back[6] = tp & 0xff;

		ZigBee_back[9] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6003);
		if (ZigBee_back[9] >= 0x80)	ZigBee_back[9] = 0xff - ZigBee_back[9];
		ZigBee_back[8] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6002);

		ExtSRAMInterface.ExMem_Write_Bytes(0x6080, ZigBee_back, 16);
		Tcount = 0x00;
	}
	else if (sendflag == 1)
	{
		Tcount += (millis() - frisrtime);
	}

	//空闲时进行流水灯
	DebugOpt::DoLEDFlow();
}