/*
 *	An Arduino library for the Hi-Link LD2410 24Ghz FMCW radar sensor.
 *
 *  This sensor is a Frequency Modulated Continuous Wave radar, which makes it good for presence detection and its sensitivity at different ranges to both static and moving targets can be configured.
 *
 *	The code in this library is based off the manufacturer datasheet and reading of this initial piece of work for ESPHome https://github.com/rain931215/ESPHome-LD2410.
 *
 *	https://github.com/ncmreynolds/ld2410
 *
 *	Released under LGPL-2.1 see https://github.com/ncmreynolds/ld2410/LICENSE for full license
 *	Modified by Robert Zmrzli
 */
#ifndef ld2410_cpp
#define ld2410_cpp
#include "ld2410.h"


ld2410::ld2410()	//Constructor function
{
}

ld2410::~ld2410()	//Destructor function
{
}

bool ld2410::begin(Stream &radarStream, bool waitForRadar)	{
	radar_uart_ = &radarStream;		//Set the stream used for the LD2410
	if(debug_uart_ != nullptr)
	{
		debug_uart_->println(F("ld2410 started"));
	}
	if(waitForRadar)
	{
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nLD2410 firmware: "));
		}
		if(requestFirmwareVersion())
		{
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F(" v"));
				debug_uart_->print(firmware_major_version);
				debug_uart_->print('.');
				debug_uart_->print(firmware_minor_version);
				debug_uart_->print('.');
				debug_uart_->print(firmware_bugfix_version);
				debug_uart_->print(F("\nEnabling engineering mode: "));
			}
		}
		if(requestStartEngineeringMode()) {
			if(debug_uart_ != nullptr) {
				debug_uart_->print(F(" E OK"));
			}
			return true;
		}
		else
		{
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F(" E no response"));
			}
		}
	}
	else
	{
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nLD2410 library configured"));
		}
		return true;
	}
	return false;
}

void ld2410::debug(Stream &terminalStream)
{
	debug_uart_ = &terminalStream;		//Set the stream used for the terminal
	#if defined(ESP8266)
	if(&terminalStream == &Serial)
	{
		  debug_uart_->write(17);			//Send an XON to stop the hung terminal after reset on ESP8266
	}
	#endif
}

bool ld2410::isConnected()
{
	if(millis() - radar_uart_last_packet_ < radar_uart_timeout)	//Use the last reading
	{
		return true;
	}
	if(read_frame_())	//Try and read a frame if the current reading is too old
	{
		return true;
	}
	return false;
}

bool ld2410::read()
{
	return read_frame_();
}

bool ld2410::presenceDetected()
{
	return target_type_ != 0;
}

bool ld2410::stationaryTargetDetected()
{
	if((target_type_ & 0x02) && stationary_target_distance_ > 0 && stationary_target_energy_ > 0)
	{
		return true;
	}
	return false;
}

uint16_t ld2410::stationaryTargetDistance()
{
	//if(stationary_target_energy_ > 0)
	{
		return stationary_target_distance_;
	}
	//return 0;
}

uint8_t ld2410::stationaryTargetEnergy()
{
	//if(stationary_target_distance_ > 0)
	{
		return stationary_target_energy_;
	}
	//return 0;
}

bool ld2410::movingTargetDetected()
{
	if((target_type_ & 0x01) && moving_target_distance_ > 0 && moving_target_energy_ > 0)
	{
		return true;
	}
	return false;
}

uint16_t ld2410::movingTargetDistance()
{
	//if(moving_target_energy_ > 0)
	{
		return moving_target_distance_;
	}
	//return 0;
}

uint8_t ld2410::movingTargetEnergy()
{
	//if(moving_target_distance_ > 0)
	{
		return moving_target_energy_;
	}
	//return 0;
}

bool ld2410::read_frame_()
{
	uint8_t byte_read_ = 0;
	if(radar_uart_ -> available())
	{
		byte_read_ = radar_uart_ -> read();

		#ifdef LD2410_DEBUG_RAW
		if(debug_uart_ != nullptr)
		{
			if(byte_read_ < 0x10)
			{
				debug_uart_->print('0');
			}
			debug_uart_->print(byte_read_, HEX);
			debug_uart_->print(' ');
		}
		#endif

		if(frame_started_ == false)
		{
			radar_data_frame_position_ = 0;
			//if(byte_read_ == 0xF4 && getting_response == 0)
			if(byte_read_ == 0xF4)
			{
				#ifdef LD2410_DEBUG_DATA
				if(debug_uart_ != nullptr)
				{
					debug_uart_->print(F("\n< : F4 "));
				}
				#endif
				radar_data_frame_[radar_data_frame_position_++] = byte_read_;
				frame_started_ = true;
				ack_frame_ = false;
			}
			else if(byte_read_ == 0xFD)
			{
				// #ifdef LD2410_DEBUG_COMMANDS
				// if(debug_uart_ != nullptr)
				// {
				// 	debug_uart_->print(F("\n< : FD "));
				// }
				// #endif
				radar_data_frame_[radar_data_frame_position_++] = byte_read_;
				frame_started_ = true;
				ack_frame_ = true;
			}
		}
		else
		{

			if(radar_data_frame_position_ < LD2410_MAX_FRAME_LENGTH)
			{

				#ifdef LD2410_DEBUG_COMMANDS
				if(debug_uart_ != nullptr && ack_frame_ == true)
				{
					if(radar_data_frame_position_ < 0x10)
					{
						debug_uart_->print('0');
					}
					debug_uart_->print(radar_data_frame_position_, HEX);
					debug_uart_->print(' ');
				}
				#endif
				radar_data_frame_[radar_data_frame_position_++] = byte_read_;
				//print_frame_();
				if(radar_data_frame_position_ > 7)	//Can check for start and end
				{
					if(	radar_data_frame_[0]                              == 0xF4 &&	//Data frame end state
						radar_data_frame_[1]                              == 0xF3 &&
						radar_data_frame_[2]                              == 0xF2 &&
						radar_data_frame_[3]                              == 0xF1 &&
						radar_data_frame_[radar_data_frame_position_ - 4] == 0xF8 &&
						radar_data_frame_[radar_data_frame_position_ - 3] == 0xF7 &&
						radar_data_frame_[radar_data_frame_position_ - 2] == 0xF6 &&
						radar_data_frame_[radar_data_frame_position_ - 1] == 0xF5
					)
					{
						//debug_uart_->print(F("\ndata frame found!"));
						if(parse_data_frame_())
						{
							#ifdef LD2410_DEBUG_DATA
							if(debug_uart_ != nullptr)
							{
								debug_uart_->print(F("parsed data OK"));
							}
							#endif
							frame_started_ = false;
							radar_data_frame_position_ = 0;
							return true;
						}
						else
						{
							#ifdef LD2410_DEBUG_DATA
							if(debug_uart_ != nullptr)
							{
								debug_uart_->print(F("failed to parse data"));
							}
							#endif
							frame_started_ = false;
							radar_data_frame_position_ = 0;
						}
					}
					else if(radar_data_frame_[0]                              == 0xFD &&	//Command frame end state
							radar_data_frame_[1]                              == 0xFC &&
							radar_data_frame_[2]                              == 0xFB &&
							radar_data_frame_[3]                              == 0xFA &&
							radar_data_frame_[radar_data_frame_position_ - 4] == 0x04 &&
							radar_data_frame_[radar_data_frame_position_ - 3] == 0x03 &&
							radar_data_frame_[radar_data_frame_position_ - 2] == 0x02 &&
							radar_data_frame_[radar_data_frame_position_ - 1] == 0x01
						)
					{
						if(parse_command_frame_())
						{
							#ifdef LD2410_DEBUG_COMMANDS
							if(debug_uart_ != nullptr)
							{
								debug_uart_->print(F("parsed command OK"));
							}
							#endif
							frame_started_ = false;
							radar_data_frame_position_ = 0;
							getting_response = 0;
							return true;
						}
						else
						{
							#ifdef LD2410_DEBUG_COMMANDS
							if(debug_uart_ != nullptr)
							{
								debug_uart_->print(F("failed to parse command"));
							}
							#endif
							frame_started_ = false;
							radar_data_frame_position_ = 0;
							getting_response = 0;
						}
					}
				}
			}
			else
			{
				#if defined(LD2410_DEBUG_DATA) || defined(LD2410_DEBUG_COMMANDS)
				if(debug_uart_ != nullptr)
				{
					debug_uart_->print(F("\nLD2410 frame overran"));
				}
				#endif
				frame_started_ = false;
				radar_data_frame_position_ = 0;
			}
		}
	}
	return false;
}

void ld2410::print_frame_()
{
	if(debug_uart_ != nullptr)
	{
		if(ack_frame_ == true)
		{
			debug_uart_->print(F("\n < Cmnd : "));
		}
		else
		{
			debug_uart_->print(F("\n < Data: "));
		}
		uint8_t frame_length = radar_data_frame_position_;
		for(uint8_t i = 0; i < frame_length; i ++)
		{
			if(radar_data_frame_[i] < 0x10)
			{
				debug_uart_->print('0');
			}
			debug_uart_->print(radar_data_frame_[i],HEX);
			debug_uart_->print(' ');
		}
		//
		debug_uart_->println("");
	}
}

void ld2410::resetGates() {

	for(uint8_t i = 0; i < 10 ; i ++)
	{
		motion_energy[i] = 0;
		stationary_energy[i] = 0;
	}
	for(uint8_t i = 10; i < 14 ; i ++)
	{
		motion_energy[i] = 0;
	}
}

bool ld2410::parse_data_frame_()
{
	uint16_t intra_frame_data_length_ = radar_data_frame_[4] + (radar_data_frame_[5] << 8);
	if(radar_data_frame_position_ == intra_frame_data_length_ + 10)
	{
		#ifdef LD2410_DEBUG_DATA
		if(debug_uart_ != nullptr && ack_frame_ == false)
		{
			debug_uart_->print(F("\n @1 : "));
			print_frame_();
		}
		else if(debug_uart_ != nullptr){
			debug_uart_->print(F("\n @2 : "));
		}

		#endif

		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr && ack_frame_ == true)
		{
			print_frame_();
		}
		#endif
		if(radar_data_frame_[6] == 0x01 && radar_data_frame_[7] == 0xAA)	//Engineering mode data
		{ //engineering report
			target_type_ = radar_data_frame_[8];
			#ifdef LD2410_DEBUG_PARSE
			if(debug_uart_ != nullptr)
			{
				//debug_uart_->print(F("\nEngineering data - "));
				if(target_type_ == 0x00)
				{
					//debug_uart_->print(F("no target"));
				}
				else if(target_type_ == 0x01)
				{
					debug_uart_->println(F("moving target:"));
				}
				else if(target_type_ == 0x02)
				{
					//debug_uart_->print(F("stationary target:"));
				}
				else if(target_type_ == 0x03)
				{
					//debug_uart_->println(F("moving & stationary targets:"));
				}
			}
			#endif
			motion_energy[0] = motion_energy[0] + 1;
			#ifdef LD2410_DEBUG_PARSE
			if(debug_uart_ != nullptr){
				debug_uart_->print("ME: ");
				debug_uart_->print( motion_energy[0]);
			}
			#endif
			motion_energy[1+target_type_] = motion_energy[1+target_type_] + 1;
			for(uint8_t gate = 1; gate <= 9; gate++) {
				motion_energy[gate+4] = motion_energy[gate+4] + radar_data_frame_[18+gate];
			}
			stationary_energy[0] = stationary_energy[0] + 1;
			for(uint8_t gate = 1; gate <= 9; gate++) {
				stationary_energy[gate] = stationary_energy[gate] + radar_data_frame_[27+gate];
			}

			radar_uart_last_packet_ = millis();
			return true;

		}
		else if(intra_frame_data_length_ == 13 && radar_data_frame_[6] == 0x02 && radar_data_frame_[7] == 0xAA && radar_data_frame_[17] == 0x55 && radar_data_frame_[18] == 0x00)	//Normal target data
		{ //normal report
			target_type_ = radar_data_frame_[8];
			//moving_target_distance_ = radar_data_frame_[9] + (radar_data_frame_[10] << 8);
			stationary_target_distance_ = radar_data_frame_[9] + (radar_data_frame_[10] << 8);
			stationary_target_energy_ = radar_data_frame_[14];
			moving_target_energy_ = radar_data_frame_[11];
			//stationary_target_distance_ = radar_data_frame_[12] + (radar_data_frame_[13] << 8);
			moving_target_distance_ = radar_data_frame_[15] + (radar_data_frame_[16] << 8);
			#ifdef LD2410_DEBUG_PARSE
			if(debug_uart_ != nullptr)
			{
				//debug_uart_->print(F("\nNormal data - "));
				if(target_type_ == 0x00)
				{
					debug_uart_->println(F("\nno target"));
				}
				else if(target_type_ == 0x01)
				{
					debug_uart_->println(F("\nmoving target:"));
				}
				else if(target_type_ == 0x02)
				{
					//debug_uart_->print(F("stationary target:"));
				}
				else if(target_type_ == 0x03)
				{
					//debug_uart_->println(F("\nmoving & stationary targets:"));
				}
				if(radar_data_frame_[8] & 0x01)
				{
					//debug_uart_->print(F(" moving at "));
					//debug_uart_->print(moving_target_distance_);
					//debug_uart_->print(F("cm power "));
					//debug_uart_->print(moving_target_energy_);
				}
				if(radar_data_frame_[8] & 0x02)
				{
					//debug_uart_->print(F(" stationary at "));
					//debug_uart_->print(stationary_target_distance_);
					//debug_uart_->print(F("cm power "));
					//debug_uart_->print(stationary_target_energy_);
				}
			}
			#endif
			radar_uart_last_packet_ = millis();
			return true;
		}
		else
		{
			#ifdef LD2410_DEBUG_DATA
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("\nUnknown frame type"));
			}
			#endif
			//print_frame_();
		}
	}
	else
	{
		#ifdef LD2410_DEBUG_DATA
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nFrame length unexpected: "));
			debug_uart_->print(radar_data_frame_position_);
			debug_uart_->print(F(" not "));
			debug_uart_->print(intra_frame_data_length_ + 10);
		}
		#endif
	}
	return false;
}

bool ld2410::parse_command_frame_()
{
	uint16_t intra_frame_data_length_ = radar_data_frame_[4] + (radar_data_frame_[5] << 8);
	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
	{
		//print_frame_();
		debug_uart_->print(F("\nACK frame payload: "));
		debug_uart_->print(intra_frame_data_length_);
		debug_uart_->print(F(" bytes"));
		debug_uart_->print(F("\nparsed command "));
	}
	#endif

	//print_frame_();
	latest_ack_ = radar_data_frame_[6];
	latest_command_success_ = (radar_data_frame_[8] == 0x00 && radar_data_frame_[9] == 0x00);
	if(intra_frame_data_length_ == 8 && latest_ack_ == 0xFF)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for entering configuration mode: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F(" OK"));
			}
			return true;
		}
		else
		{
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F(" failed"));
			}
			return false;
		}
	}
	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xFE)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for leaving configuration mode: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			return true;
		}
		else
		{
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			return false;
		}
	}
	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x60)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for setting max values: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			return true;
		}
		else
		{
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			return false;
		}
	}
	else if(intra_frame_data_length_ == 28 && latest_ack_ == 0x61)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for current configuration: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			max_gate = radar_data_frame_[11];
			max_moving_gate = radar_data_frame_[12];
			max_stationary_gate = radar_data_frame_[13];
			motion_sensitivity[0] = radar_data_frame_[14];
			motion_sensitivity[1] = radar_data_frame_[15];
			motion_sensitivity[2] = radar_data_frame_[16];
			motion_sensitivity[3] = radar_data_frame_[17];
			motion_sensitivity[4] = radar_data_frame_[18];
			motion_sensitivity[5] = radar_data_frame_[19];
			motion_sensitivity[6] = radar_data_frame_[20];
			motion_sensitivity[7] = radar_data_frame_[21];
			motion_sensitivity[8] = radar_data_frame_[22];
			stationary_sensitivity[0] = radar_data_frame_[23];
			stationary_sensitivity[1] = radar_data_frame_[24];
			stationary_sensitivity[2] = radar_data_frame_[25];
			stationary_sensitivity[3] = radar_data_frame_[26];
			stationary_sensitivity[4] = radar_data_frame_[27];
			stationary_sensitivity[5] = radar_data_frame_[28];
			stationary_sensitivity[6] = radar_data_frame_[29];
			stationary_sensitivity[7] = radar_data_frame_[30];
			stationary_sensitivity[8] = radar_data_frame_[31];
			sensor_idle_time = radar_data_frame_[32];
			sensor_idle_time += (radar_data_frame_[33] << 8);
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("\nMax gate distance: "));
				debug_uart_->print(max_gate);
				debug_uart_->print(F("\nMax motion detecting gate distance: "));
				debug_uart_->print(max_moving_gate);
				debug_uart_->print(F("\nMax stationary detecting gate distance: "));
				debug_uart_->print(max_stationary_gate);
				debug_uart_->print(F("\nSensitivity per gate"));
				for(uint8_t i = 0; i < 9; i++)
				{
					debug_uart_->print(F("\nGate "));
					debug_uart_->print(i);
					debug_uart_->print(F(" ("));
					debug_uart_->print(i * 0.75);
					debug_uart_->print('-');
					debug_uart_->print((i+1) * 0.75);
					debug_uart_->print(F(" metres) Motion: "));
					debug_uart_->print(motion_sensitivity[i]);
					debug_uart_->print(F(" Stationary: "));
					debug_uart_->print(stationary_sensitivity[i]);

				}
				debug_uart_->print(F("\nSensor idle timeout: "));
				debug_uart_->print(sensor_idle_time);
				debug_uart_->print('s');
			}
			#endif
			return true;
		}
		else
		{
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			#endif
			return false;
		}
	}
	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x64)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for setting sensitivity values: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			return true;
		}
		else
		{
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			#endif
			return false;
		}
	}
	else if(intra_frame_data_length_ == 12 && latest_ack_ == 0xA0)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for firmware version: "));
		}
		#endif
		if(latest_command_success_)
		{
			firmware_major_version = radar_data_frame_[13];
			firmware_minor_version = radar_data_frame_[12];
			firmware_bugfix_version = radar_data_frame_[14];
			firmware_bugfix_version += radar_data_frame_[15]<<8;
			firmware_bugfix_version += radar_data_frame_[16]<<16;
			firmware_bugfix_version += radar_data_frame_[17]<<24;
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			return true;
		}
		else
		{
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			#endif
			return false;
		}
	}
	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xA2)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for factory reset: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			return true;
		}
		else
		{
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			#endif
			return false;
		}
	}
	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0xA3)
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nACK for restart: "));
		}
		#endif
		if(latest_command_success_)
		{
			radar_uart_last_packet_ = millis();
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("OK"));
			}
			#endif
			return true;
		}
		else
		{
			#ifdef LD2410_DEBUG_COMMANDS
			if(debug_uart_ != nullptr)
			{
				debug_uart_->print(F("failed"));
			}
			#endif
			return false;
		}
	}
	else if(intra_frame_data_length_ == 4 && latest_ack_ == 0x62)
	{
		return true;
	}
	else
	{
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\nUnknown ACK"));
		}
		#endif
	}
	return false;
}

void ld2410::SendToRadar(uint8_t to_send) {
	radar_uart_->write((byte)to_send);

	// #ifdef LD2410_DEBUG_COMMANDS
	// if(debug_uart_ != nullptr)
	// {
	// 	if(to_send < 0x10)
	// 	{
	// 		debug_uart_->print('0');
	// 	}
	// 	debug_uart_->print(to_send, HEX);
	// 	debug_uart_->print(' ');
	// }
	// #endif
}

void ld2410::send_command_preamble_()
{
	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
		debug_uart_->print("Sending: ");
	#endif
	//Command preamble
	radar_uart_->write((byte)0xFD);

	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" FD");
	}
	#endif

	radar_uart_->write((byte)0xFC);
	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" FC");
	}
	#endif
	radar_uart_->write((byte)0xFB);
	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" FB");
	}
	#endif
	radar_uart_->write((byte)0xFA);
	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" FA");
	}
	#endif
}

void ld2410::send_command_postamble_()
{
	//Command end
	radar_uart_->write((byte)0x04);
	#ifdef LD2410_DEBUG_COMMANDS
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 04");
	}
	#endif
	radar_uart_->write((byte)0x03);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 03");
	}

	radar_uart_->write((byte)0x02);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 02");
	}

	radar_uart_->write((byte)0x01);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 01");
	}
}


bool ld2410::enter_configuration_mode_()
{
	//Serial.print(F("\nenter_configuration_mode_"));
	send_command_preamble_();
	//Request firmware
	radar_uart_->write((byte) 0x04);	//Command is four bytes long
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 04");
	}
	radar_uart_->write((byte) 0x00);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 00");
	}
	radar_uart_->write((byte) 0xFF);	//Request enter command mode
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" FF");
	}
	radar_uart_->write((byte) 0x00);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 00");
	}
	radar_uart_->write((byte) 0x01);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 01");
	}
	radar_uart_->write((byte) 0x00);
	if(debug_uart_ != nullptr)
	{
		debug_uart_->print(" 00");
	}
	send_command_postamble_();
	frame_started_ = false;
	radar_data_frame_position_ = 0;
	radar_uart_last_command_ = millis();
	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
	{
		if(read_frame_())
		{
			//debug_uart_->print(F("\nFrame read!"));
			if(latest_ack_ == 0xFF && latest_command_success_)
			{
				//Serial.println(" OK");
				return true;
			}
		}
	}

	//Serial.println(" NOK");
	return false;
}

bool ld2410::leave_configuration_mode_()
{
	//Serial.print(F("\nleave_configuration_mode_"));
	send_command_preamble_();
	//Request firmware
	radar_uart_->write((byte) 0x02);	//Command is four bytes long
	radar_uart_->write((byte) 0x00);
	radar_uart_->write((byte) 0xFE);	//Request leave command mode
	radar_uart_->write((byte) 0x00);
	send_command_postamble_();
	radar_uart_last_command_ = millis();
	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
	{
		if(read_frame_())
		{
			if(latest_ack_ == 0xFE && latest_command_success_)
			{
				//Serial.println(" OK");
				return true;
			}
		}
	}
	//Serial.println(" NOK");
	return false;
}

bool ld2410::requestStartEngineeringMode()
{

	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		//Request firmware
		radar_uart_->write((byte) 0x02);	//Command is four bytes long
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(" 02");
		}
		radar_uart_->write((byte) 0x00);
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(" 00");
		}
		radar_uart_->write((byte) 0x62);	//Request enter command mode
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(" 62");
		}
		radar_uart_->write((byte) 0x00);
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(" 00");
		}
		send_command_postamble_();
		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			if(read_frame_())
			{
				if(latest_ack_ == 0x62 && latest_command_success_)
				{
					delay(50);
					leave_configuration_mode_();
					return true;
				}
			}
		}
	}
	else {
		delay(50);
		leave_configuration_mode_();
		return false;
	}

	delay(50);
	leave_configuration_mode_();
	return false;
}

bool ld2410::requestEndEngineeringMode()
{
	send_command_preamble_();
	//Request firmware
	radar_uart_->write((byte) 0x02);	//Command is four bytes long
	radar_uart_->write((byte) 0x00);
	radar_uart_->write((byte) 0x63);	//Request leave command mode
	radar_uart_->write((byte) 0x00);
	send_command_postamble_();
	radar_uart_last_command_ = millis();
	while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
	{
		if(read_frame_())
		{
			if(latest_ack_ == 0x63 && latest_command_success_)
			{
				return true;
			}
		}
	}
	return false;
}

void ld2410::Marker(uint8_t state)
{
	radar_uart_->write((byte)0xAA);
	radar_uart_->write((byte)state);
	radar_uart_->write((byte)0x55);
}


bool ld2410::requestCurrentConfiguration()
{
	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		//Request firmware
		SendToRadar(0x02);	//Command is two bytes long
		SendToRadar(0x00);
		SendToRadar(0x61);	//Request current configuration
		SendToRadar(0x00);
		send_command_postamble_();
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\n> Out : FDFCFBFA 02 00 61 00 04030201"));
		}
		#endif

		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			if(read_frame_())
			{
				if(latest_ack_ == 0x61 && latest_command_success_)
				{
					delay(50);
					leave_configuration_mode_();
					return true;
				}
			}
		}
	}
	delay(50);
	leave_configuration_mode_();
	return false;
}

bool ld2410::requestFirmwareVersion()
{
	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		//Request firmware
		SendToRadar(0x02);	//Command is two bytes long
		SendToRadar(0x00);
		SendToRadar(0xA0);	//Request firmware version
		SendToRadar(0x00);
		send_command_postamble_();
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\n> Out : FDFCFBFA 02 00 A0 00 04030201"));
		}
		#endif

		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			read_frame_();
			if(latest_ack_ == 0xA0 && latest_command_success_)
			{
				delay(50);
				leave_configuration_mode_();
				return true;
			}
		}
	}
	delay(50);
	leave_configuration_mode_();
	return false;
}

bool ld2410::requestRestart()
{
	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		//Request firmware
		SendToRadar(0x02);	//Command is two bytes long
		SendToRadar(0x00);
		SendToRadar(0xA3);	//Request restart
		SendToRadar(0x00);
		send_command_postamble_();
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\n> Out : FDFCFBFA 02 00 A3 00 04030201"));
		}
		#endif

		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			if(read_frame_())
			{
				if(latest_ack_ == 0xA3 && latest_command_success_)
				{
					delay(50);
					leave_configuration_mode_();
					return true;
				}
			}
		}
	}
	delay(50);
	leave_configuration_mode_();
	return false;
}

bool ld2410::requestFactoryReset()
{
	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		//Request firmware
		SendToRadar(0x02);	//Command is two bytes long
		SendToRadar(0x00);
		SendToRadar(0xA2);	//Request factory reset
		SendToRadar(0x00);
		send_command_postamble_();
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\n> Out : FDFCFBFA 02 00 A2 00 04030201"));
		}
		#endif

		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			if(read_frame_())
			{
				if(latest_ack_ == 0xA2 && latest_command_success_)
				{
					delay(50);
					leave_configuration_mode_();
					return true;
				}
			}
		}
	}
	delay(50);
	leave_configuration_mode_();
	return false;
}

bool ld2410::setMaxValues(uint16_t moving, uint16_t stationary, uint16_t inactivityTimer)
{
	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		SendToRadar(0x14);	//Command is 20 bytes long
		SendToRadar(0x00);
		SendToRadar(0x60);	//Request set max values
		SendToRadar(0x00);
		SendToRadar(0x00);	//Moving gate command
		SendToRadar(0x00);
		SendToRadar(char(moving & 0x00FF));	//Moving gate value
		SendToRadar(char((moving & 0xFF00)>>8));
		SendToRadar(0x00);	//Spacer
		SendToRadar(0x00);
		SendToRadar(0x01);	//Stationary gate command
		SendToRadar(0x00);
		SendToRadar(char(stationary & 0x00FF));	//Stationary gate value
		SendToRadar(char((stationary & 0xFF00)>>8));
		SendToRadar(0x00);	//Spacer
		SendToRadar(0x00);
		SendToRadar(0x02);	//Inactivity timer command
		SendToRadar(0x00);
		SendToRadar(char(inactivityTimer & 0x00FF));	//Inactivity timer
		SendToRadar(char((inactivityTimer & 0xFF00)>>8));
		SendToRadar(0x00);	//Spacer
		SendToRadar(0x00);
		send_command_postamble_();
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\n> Out : A lot!!!"));
		}
		#endif

		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			if(read_frame_())
			{
				if(latest_ack_ == 0x60 && latest_command_success_)
				{
					delay(50);
					leave_configuration_mode_();
					return true;
				}
			}
		}
	}
	delay(50);
	leave_configuration_mode_();
	return false;
}

bool ld2410::setGateSensitivityThreshold(uint8_t gate, uint8_t moving, uint8_t stationary)
{
	if(enter_configuration_mode_())
	{
		delay(50);
		send_command_preamble_();
		SendToRadar(0x14);	//Command is 20 bytes long
		SendToRadar(0x00);
		SendToRadar(0x64);	//Request set sensitivity values
		SendToRadar(0x00);
		SendToRadar(0x00);	//Gate command
		SendToRadar(0x00);
		SendToRadar(char(gate));	//Gate value
		SendToRadar(0x00);
		SendToRadar(0x00);	//Spacer
		SendToRadar(0x00);
		SendToRadar(0x01);	//Motion sensitivity command
		SendToRadar(0x00);
		SendToRadar(char(moving));	//Motion sensitivity value
		SendToRadar(0x00);
		SendToRadar(0x00);	//Spacer
		SendToRadar(0x00);
		SendToRadar(0x02);	//Stationary sensitivity command
		SendToRadar(0x00);
		SendToRadar(char(stationary));	//Stationary sensitivity value
		SendToRadar(0x00);
		SendToRadar(0x00);	//Spacer
		SendToRadar(0x00);
		send_command_postamble_();
		#ifdef LD2410_DEBUG_COMMANDS
		if(debug_uart_ != nullptr)
		{
			debug_uart_->print(F("\n> Out : Also a lot!!!"));
		}
		#endif

		radar_uart_last_command_ = millis();
		while(millis() - radar_uart_last_command_ < radar_uart_command_timeout_)
		{
			if(read_frame_())
			{
				if(latest_ack_ == 0x64 && latest_command_success_)
				{
					delay(50);
					leave_configuration_mode_();
					return true;
				}
			}
		}
	}
	delay(50);
	leave_configuration_mode_();
	return false;
}
#endif