#include "RealSenseDevice.h"
#include "PCH.h"
#include <librealsense2/rs_advanced_mode.hpp>

URealSenseDevice::URealSenseDevice(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
	REALSENSE_TRACE(TEXT("+URealSenseDevice_%p"), this);
}

URealSenseDevice::~URealSenseDevice()
{
	REALSENSE_TRACE(TEXT("~URealSenseDevice_%p"), this);

	SetHandle(nullptr);
}

void URealSenseDevice::SetHandle(rs2_device* Handle)
{
	if (RsDevice) { rs2_delete_device(RsDevice); }
	RsDevice = Handle;
}

rs2_device* URealSenseDevice::GetHandle()
{
	return RsDevice;
}

URealSenseSensor* URealSenseDevice::NewSensor(rs2_sensor* Handle, const TCHAR* ObjName)
{
	auto Sensor = NewNamedObject<URealSenseSensor>(this, ObjName);
	Sensor->SetHandle(Handle);
	return Sensor;
}

void URealSenseDevice::QueryData()
{
	Sensors.Empty();

	rs2::error_ref e;
	Name = uestr(rs2_get_device_info(RsDevice, RS2_CAMERA_INFO_NAME, &e));
	Serial = uestr(rs2_get_device_info(RsDevice, RS2_CAMERA_INFO_SERIAL_NUMBER, &e));

	rs2_sensor_list* List = rs2_query_sensors(RsDevice, &e);
	if (List)
	{
		const int Count = rs2_get_sensors_count(List, &e);
		for (int i = 0; i < Count; i++)
		{
			rs2_sensor* RsSensor = rs2_create_sensor(List, i, &e);
			if (RsSensor)
			{
				Sensors.Add(NewSensor(RsSensor, TEXT("RealSenseSensor")));
			}
		}

		rs2_delete_sensor_list(List);
	}

	for (auto* Sensor : Sensors)
	{
		Sensor->QueryData();
	}
}

URealSenseSensor* URealSenseDevice::GetSensor(ERealSenseStreamType StreamType)
{
	URealSenseSensor* Result = nullptr;

	for (auto* Sensor : Sensors)
	{
		for (const auto& Profile : Sensor->StreamProfiles)
		{
			if (Profile.StreamType == StreamType || StreamType == ERealSenseStreamType::STREAM_ANY)
			{
				Result = Sensor;
				break;
			}
		}
	}

	return Result;
}

TArray<FRealSenseStreamProfile> URealSenseDevice::GetStreamProfiles(ERealSenseStreamType StreamType)
{
	TArray<FRealSenseStreamProfile> Result;

	for (auto* Sensor : Sensors)
	{
		for (const auto& Profile : Sensor->StreamProfiles)
		{
			if (Profile.StreamType == StreamType || StreamType == ERealSenseStreamType::STREAM_ANY)
			{
				Result.Add(Profile);
			}
		}
	}

	return Result;
}

bool URealSenseDevice::SupportsProfile(FRealSenseStreamProfile Profile)
{
	for (auto* Sensor : Sensors)
	{
		if (Sensor->SupportsProfile(Profile))
		{
			return true;
		}
	}

	return false;
}

bool URealSenseDevice::LoadPreset(const FString& FileName)
{
	return LoadPreset(RsDevice, FileName);
}

bool URealSenseDevice::SavePreset(const FString& FileName)
{
	return SavePreset(RsDevice, FileName);
}

bool URealSenseDevice::LoadPreset(rs2_device* Handle, const FString& FileName)
{
	bool Result = false;
	IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile();
	IFileHandle* FileHandle = PlatformFile.OpenRead(*FileName);
	if (!FileHandle)
	{
		REALSENSE_ERR(TEXT("OpenRead failed: %s"), *FileName);
	}
	else
	{
		auto Size = FileHandle->Size();
		std::vector<uint8> Data;
		Data.resize(Size);

		if (!FileHandle->Read(&Data[0], Size))
		{
			REALSENSE_ERR(TEXT("Read failed: %s Size=%u"), *FileName, (unsigned int)Size);
		}
		else
		{
			rs2::error_ref e;
			rs2_load_json(Handle, &Data[0], Size, &e);
			if (!e.success())
			{
				REALSENSE_ERR(TEXT("rs2_load_json failed: %s"), ANSI_TO_TCHAR(e.get_message()));
			}
			else
			{
				Result = true;
			}
		}

		delete FileHandle;
	}
	return Result;
}

bool URealSenseDevice::SavePreset(rs2_device* Handle, const FString& FileName)
{
	bool Result = false;
	IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile();
	IFileHandle* FileHandle = PlatformFile.OpenWrite(*FileName);
	if (!FileHandle)
	{
		REALSENSE_ERR(TEXT("OpenWrite failed: %s"), *FileName);
	}
	else
	{
		rs2::error_ref e;
		auto Buffer = rs2_serialize_json(Handle, &e);
		if (!e.success())
		{
			REALSENSE_ERR(TEXT("rs2_serialize_json failed: %s"), ANSI_TO_TCHAR(e.get_message()));
		}
		else
		{
			auto Size = rs2_get_raw_data_size(Buffer, &e);
			auto Data = rs2_get_raw_data(Buffer, &e);

			if (!FileHandle->Write(Data, Size))
			{
				REALSENSE_ERR(TEXT("Write failed: %s"), *FileName);
			}
			else
			{
				Result = true;
			}

			rs2_delete_raw_data(Buffer);
		}

		delete FileHandle;
	}
	return Result;
}
