// 
// 标志物通信 - 路灯 - IR
// 

#include "RodeLightCommand.h"

void RodeLightCommand::Swap(uint16_t& a, uint16_t& b)
{
	uint16_t c = a;
	a = b;
	b = c;
}

byte RodeLightCommand::CalcLevel(uint16_t* sorted_arr, uint16_t curr)
{
	if (curr >= sorted_arr[3])
		return 4;
	else if (curr >= sorted_arr[2])
		return 3;
	else if (curr >= sorted_arr[1])
		return 2;
	else
		return 1;
}

RodeLightCommand::RodeLightCommand()
{
	ir_cmd[0] = 0x00;
	ir_cmd[1] = 0xFF;
	ir_cmd[2] = 0x00;
	ir_cmd[3] = 0x00;
}

byte* RodeLightCommand::CMD_LightPlusOne()
{
	ir_cmd[2] = 0x0C;
	ir_cmd[3] = 0xF3;
	return ir_cmd;
}

byte* RodeLightCommand::CMD_LightPlusTwo()
{
	ir_cmd[2] = 0x18;
	ir_cmd[3] = 0xE7;
	return ir_cmd;
}

byte* RodeLightCommand::CMD_LightPlusThree()
{
	ir_cmd[2] = 0x5E;
	ir_cmd[3] = 0xA1;
	return ir_cmd;
}

byte RodeLightCommand::GetCurrentLevel()
{
	RodeLightCommand cmd;
	uint16_t light_arr[4] = { 0 };
	uint16_t current = BH1750.ReadLightLevel(), last = 0;
	for (int i = 0; i < 4; i++)
	{
		Infrare.Transmition(cmd.CMD_LightPlusOne(), 4);
		light_arr[i] = BH1750.ReadLightLevel(true);
		while(abs(last - light_arr[i]) <= 200)
		{
			Infrare.Transmition(cmd.CMD_LightPlusOne(), 4);
			light_arr[i] = BH1750.ReadLightLevel(true);
			delay(1500);
		}
		last = light_arr[i];
		delay(1500);
	}
	for (int i = 0; i < 4; i++)
		for (int j = 0; j < 4; j++)
			if (light_arr[i] < light_arr[j])
				Swap(light_arr[i], light_arr[j]);
	/*DataTool::PrintDataArray(light_arr, 4);
	Serial.print("Current: ");
	Serial.println(current, HEX);*/
	return CalcLevel(light_arr, current);
}

void RodeLightCommand::SetToSpeclevel(byte target_lv, byte current_lv)
{
	uint8_t times = 0;
	RodeLightCommand cmd;
	if (target_lv > current_lv)
		times = target_lv - current_lv;
	else
		times = 4 - (current_lv - target_lv);
	switch (times)
	{
	case 1:
		Infrare.Transmition(cmd.CMD_LightPlusOne(), 4);
		break;
	case 2:
		Infrare.Transmition(cmd.CMD_LightPlusTwo(), 4);
		break;
	case 3:
		Infrare.Transmition(cmd.CMD_LightPlusThree(), 4);
		break;
	default:
		return;
	}
}