///////Header//////////

// Fill out your copyright notice in the Description page of Project Settings.

#pragma once

#include "GameFramework/PlayerController.h"
#include <rplidar.h> //RPLIDAR standard sdk, all-in-one header
#include "LidarPlayerController.generated.h"


#ifndef _countof
#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif

//using namespace rp::standalone::rplidar;

/**
 * 
 */
UCLASS()
class OCULUSTEST_API ALidarPlayerController : public APlayerController
{
	GENERATED_BODY()
	
private:
	const char * opt_com_path = nullptr;
	_u32         opt_com_baudrate = 115200;
	u_result     op_result;
	rp::standalone::rplidar::RPlidarDriver * drv;
	bool checkRPLIDARHealth(rp::standalone::rplidar::RPlidarDriver * driver);

public:
	ALidarPlayerController(const class FObjectInitializer& PCIP);
	~ALidarPlayerController();
	FVector2D poll();
};


///////CPP//////////

ALidarPlayerController::ALidarPlayerController(const class FObjectInitializer& PCIP) : Super(PCIP) 
{
	opt_com_path = "\\\\.\\com3"; //Windows specific

	// create the driver instance
	drv = rp::standalone::rplidar::RPlidarDriver::CreateDriver(rp::standalone::rplidar::RPlidarDriver::DRIVER_TYPE_SERIALPORT);

	if (!drv) {
		if (GEngine)
			GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "insufficent memory, exit\n");
		exit(-2);
	}


	// make connection...
	if (IS_FAIL(drv->connect(opt_com_path, opt_com_baudrate))) {
		if (GEngine)
			GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Error, cannot bind to the specified serial port %s.\n");
	}

	// check health...
	if (!checkRPLIDARHealth(drv)) {
		if (GEngine)
			GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Lidar health warning.");
	}


	// start scan...
	drv->startScan();

	if (GEngine)
		GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Lidar loaded\n");
};
ALidarPlayerController::~ALidarPlayerController()
{
	rp::standalone::rplidar::RPlidarDriver::DisposeDriver(drv);
};


FVector2D ALidarPlayerController::poll()
{
	rplidar_response_measurement_node_t nodes[360];
	size_t   count = _countof(nodes);
	printf("%u\n", count);
	op_result = drv->grabScanData(nodes, count);

	if (IS_OK(op_result)) {
		drv->ascendScanData(nodes, count);
		for (int pos = 0; pos < (int)count; ++pos) {
			float angle = (nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
			float distance = nodes[pos].distance_q2 / 4.0f;
		}
	}
	return FVector2D(0.0f, 0.0f);
}

bool ALidarPlayerController::checkRPLIDARHealth(rp::standalone::rplidar::RPlidarDriver * driver)
{

	u_result     op_result;
	rplidar_response_device_health_t healthinfo;


	op_result = drv->getHealth(healthinfo);
	if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
		if (GEngine)
			GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "RPLidar health status : %d\n");
		if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
			if (GEngine)
				GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Error, rplidar internal error detected. Please reboot the device to retry.\n");
			// enable the following code if you want rplidar to be reboot by software
			// driver->reset();
			return false;
		} else {
			return true;
		}

	} else {
		if (GEngine)
			GEngine->AddOnScreenDebugMessage(-1, 5.f, FColor::Yellow, "Error, cannot retrieve the lidar health code: %x\n");
		return false;
	}
}


///////////Project.Build.cs///////////////////

// Copyright 1998-2014 Epic Games, Inc. All Rights Reserved.

using UnrealBuildTool;
using System.IO;
public class OculusTest : ModuleRules
{
    private string ModulePath
    {
        get { return Path.GetDirectoryName(RulesCompiler.GetModuleFilename(this.GetType().Name)); }
    }

    private string ThirdPartyPath
    {
        get { return Path.GetFullPath(Path.Combine( ModulePath, "../../ThirdParty/")); }
    }


	public OculusTest(TargetInfo Target)
	{
		PublicDependencyModuleNames.AddRange(new string[] { "Core", "CoreUObject", "Engine", "InputCore" });
        // Add static libs
        PublicAdditionalLibraries.Add(Path.Combine(ThirdPartyPath, "RPLidar/Libraries", "rplidar_driver.lib"));
        // Add a path with the .h that has the function definitions
        PublicIncludePaths.Add(Path.Combine(ThirdPartyPath, "RPLidar", "Includes"));
    }
}