#include <rpos/robot_platforms/slamware_core_platform.h>

struct CamAttr
{
	float minValidDistance;
	float maxValidDistance;
	float minFovPitch;
	float maxFovPitch;
	float minFovYaw;
	float maxFovYaw;
	int cols;
	int rows;
};



int main(int argc, char* argv[])
{
	try
	{
		rpos::robot_platforms::SlamwareCorePlatform platform = rpos::robot_platforms::SlamwareCorePlatform::connect("192.168.11.1", 1445);
		
		CamAttr camera_attr;
		//getAttrFromDevice(camera_attr); // TODO: Get attr from device doc maybe

		rpos::message::depth_camera::DepthCameraFrame frame;         
		frame.minValidDistance = 0.4f;                   //frame.minValidDistance = camera_attr.minValidDistance;
		frame.maxValidDistance = 2.0f;
		frame.minFovPitch      = -45/2*3.14/180;
		frame.maxFovPitch      = 45/2*3.14/180;
		frame.minFovYaw        = -58/2*3.14/180;
		frame.maxFovYaw        = 58/2*3.14/180;
		frame.cols             = 320;
		frame.rows             = 240;

		
		std::vector<float> frame_buffer(76800,0);
		//getFrameFromDepthCam(frame_buffer); // TODO: Get Frame buffer from DepthCam Device

		for(unsigned long j=38081; j<38401;j++)
		{
			frame_buffer[j]=1.0f;
			//printf("frame_buffer_pDepth[%lu]=%f\r\n",j,frame_buffer[j]);
		}

		for(unsigned long i = 0; i <76800; i++)
		{   		 
		     	frame.data.push_back(frame_buffer[i]);
		     	printf("pDepth[%lu]=%f\r\n",i,frame.data[i]);
		}

		//frame.data.swap(frame_buffer);
		
		int sensorId = 5; //TODO: Config your own SensorId
		// Publish one single frame

		while(true)
		{
		platform.publishDepthCamFrame(sensorId, frame);
		}
	}
	catch(const std::exception& ex)
	{
		std::string ex_str(ex.what());
	}
	return 0;
}