#include "IntelRealSenseControl.h"
#include <librealsense2/rs.hpp>
#include <librealsense2/rsutil.h>
#include <iostream>

void IntelRealSenseControl::start()
{
	const auto lc_errorCallback = [&](rs2_log_severity f_severity, rs2::log_message const& fcr_msg) {
		if (f_severity == RS2_LOG_SEVERITY_WARN)
		{
			std::cout << f_severity << ' ' << fcr_msg.full() << std::endl;
		}
		else if (f_severity == RS2_LOG_SEVERITY_ERROR || f_severity == RS2_LOG_SEVERITY_FATAL)
		{
			std::cout << f_severity << ' ' << fcr_msg.full() << std::endl;
		}
		else
		{
			//std::cout << f_severity << ' ' << fcr_msg.full() << std::endl;
		}
	};

	rs2::log_to_callback(RS2_LOG_SEVERITY_ALL, lc_errorCallback);

	m_isRunning = true;
	m_restart = true;

	// start thread to sensor control loop
	m_controlThread = std::thread(&IntelRealSenseControl::controlLoop, this);
}

void IntelRealSenseControl::stop()
{
	if (!m_isRunning) return;
	m_isRunning = false;
	if (m_controlThread.joinable())
		m_controlThread.join();
	return;
}

void IntelRealSenseControl::controlLoop()
{
	long long l_waitDurMs = 2000;
	while (m_isRunning)
	{
		if (m_restart)
		{
			
			std::this_thread::sleep_for<long long, std::milli>(std::chrono::milliseconds(l_waitDurMs));
			if (mp_sensorManager) {
				mp_sensorManager->stop();
				mp_sensorManager.reset();
			}
			mp_sensorManager = std::make_unique<IntelRealSenseSensor>();
			mp_sensorManager->start(m_restart, createCallBack());
			m_restart = false;
			std::this_thread::sleep_for<long long, std::milli>(std::chrono::milliseconds(l_waitDurMs));
		}
		std::this_thread::sleep_for<long long, std::milli>(std::chrono::milliseconds(l_waitDurMs));
	}

	if (mp_sensorManager) {
		mp_sensorManager->stop();
		std::cout << " Final sensor destructor call " << std::endl;
		mp_sensorManager.reset();
	}
}

std::function<void(rs2::frame)> IntelRealSenseControl::createCallBack()
{
	rs2_vector gyro_data;

	return [&](rs2::frame frame) {

		if (!m_isRunning || m_restart) return;

		// frame is a motion_frame for RS2_STREAM_ACCEL and RS2_STREAM_GYRO
		if (RS2_STREAM_GYRO == frame.get_profile().stream_type())
		{


			// get angular velocity from gyroscope
			rs2::motion_frame motion_frame = frame;
			gyro_data = motion_frame.get_motion_data();
			std::cout << " gyro stream data : " << gyro_data.x << " " << gyro_data.y << " " << gyro_data.z << std::endl;
			return;
		}

	};
}