#ifndef MYCAM_OBJ_H
#define MYCAM_OBJ_H

#include "FlyCapture2.h"

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/opencv.hpp>

#include <iostream>
#include <sstream>
#include <time.h>
#include <string>

#include "Info_print.h"
#include "myGraphic.h"
#include "Detection_classes.h"
#include "myPrinter.h"
#include "myToolpath.h"
#include "myTarget.h"
#include "myPCA.h"

class MyCam {
	
	// Debug (show image) or not 
	const bool DEBUG = 1;

	clock_t myTimer;
	clock_t getPosTime;
	double lastStamp_ms = 0.;
	double currentStamp_ms = 0.;
	double stampDuration_ms = 0.;

	double lastEmbeddedStamp = 0.;
	double currentEmbeddedStamp = 0.;

	// FlyCapture camera object
	FlyCapture2::Camera cam;
	// FlyCapture camera info
	FlyCapture2::CameraInfo camInfo;
	// FlyCapture camera error info
	FlyCapture2::Error error;

	// For real-time image storage
	FlyCapture2::Image rawImage;
	FlyCapture2::Image rgbImage;
	unsigned int rowBytes;

	// Target tracking info
	const int window_size_h = 60; //30, 9pnt, 7ms total detection

	// Real-time tracking
	///////////////////////// debug accuracy of C_c2p ////////////////////
	cv::Mat last_p_t2p = cv::Mat::zeros(3, 1, CV_64FC1);
	cv::Mat last_cam_mot_p = cv::Mat::zeros(3, 1, CV_64FC1);
	/////////////////////////////////////////////////////////////////////
	cv::Mat current_cmd;
	//std::vector<double> current_cmd_vec = std::vector<double>(3);
	//std::vector<double> speed = { 45., 45., 45. }; // mm/s, initialized, to be changed

	// For P controller
	double inter_waypnt_ratio;
	cv::Mat desiredWaypnt;
	cv::Mat tracking_err_prt;
	std::queue<cv::Mat> tracking_avg_window;
	cv::Mat tracking_avg_window_sum = cv::Mat::zeros(3,1, CV_64FC1);
	const int moving_avg_filter_size = 5;


public:
	cv::Mat p_t2p;

	std::vector<double> current_cmd_vec = std::vector<double>(3);
	std::vector<double> speed = { 45., 45., 45. }; // mm/s, initialized, to be changed

	cv::Mat image;

	// Cam calib results
	cv::Mat intrinsic = cv::Mat(3, 3, CV_32FC1);
	cv::Mat distCoeffs;
	cv::Mat projection_matrix;
	std::vector<cv::Point3d> feature3D;
	cv::Mat tip_c = cv::Mat::zeros(3, 1, CV_64FC1);
	std::vector<cv::Point2f> tip_c_imgpts = std::vector<cv::Point2f>(1);

	// hand-eye calib results
	cv::Mat C_c2p;
	std::vector<std::vector<cv::Point2f>> chess_corners; // recognized features
	std::vector<std::vector<cv::Point3f>> chess_obj; // prior knowledge of features


	// For Target (marker) init
	MyTarget myTarget;
	//// Target info
	//const int pnt_num = 8;
	//std::vector<myKeyPnt> keyPnts = std::vector<myKeyPnt>(pnt_num);
	//std::vector<cv::Point2f> keyPnts_img = std::vector<cv::Point2f>(pnt_num);
	//// prior knowledge of features
	//std::vector<cv::Point3f> target_obj = { cv::Point3f(-14.09,14.09,0.), cv::Point3f(0., 14.09, 0.), cv::Point3f(14.09, 14.09, 0.), cv::Point3f(14.09, 0., 0.), cv::Point3f(14.09, -14.09, 0.), cv::Point3f(0., -14.09, 0.), cv::Point3f(-14.09, -14.09, 0.), cv::Point3f(-14.09, 0., 0.)};
	//std::vector<cv::Point3f> target_axis = { cv::Point3f(0., 0., 0.), cv::Point3f(20., 0., 0.),cv::Point3f(0., 20., 0.),cv::Point3f(0., 0., 20.) }; // o-x-y-z axes, mm

	// position states (o2c)
	cv::Mat target_tvecs;
	cv::Mat target_rvecs;
	cv::Mat target_rmats;
	// position states (o2p)
	cv::Mat p_o2p;
	// velocity states (o2p)
	cv::Mat last_p_o2p;
	cv::Mat v_o2p = cv::Mat::zeros(3, 1, CV_64FC1);
	const double v_o2p_thres = 0.3; // mm/s, based on noise level
	// velocity moving avg filter
	cv::Mat dp_o2p;
	std::queue<cv::Mat> dp_o2p_avg_window;
	cv::Mat dp_o2p_avg_window_sum = cv::Mat::zeros(3, 1, CV_64FC1);
	std::queue<std::vector<double>> angular_vel_avg_window;
	std::vector<double> angular_vel_avg_window_sum{0.,0.,0.};
	const int dp_o2p_filter_size = 8;
	std::queue<double> dt_avg_window;
	double dt_avg_window_sum = 0.;
	// angular velocity state
	const double w_thres = 0.04; // rad/s, based on noise level
	std::vector<double> quat{ 0.,1.,0.,0. };
	std::vector<double> last_quat{ 0.,1.,0.,0. };
	std::vector<double> last_quat_conj{ 0.,1.,0.,0. };
	std::vector<double> next_quat{ 0.,1.,0.,0. };
	std::vector<double> d_quat{ 0.,1.,0.,0. };
	std::vector<double> w{ 0.,0.,0. };
	// Data logging
	std::vector<cv::Mat> v_o2p_mov_avg;
	std::vector<cv::Mat> v_o2p_raw;
	std::vector<double> t_duration_raw;
	std::vector<cv::Mat> dp_o2p_raw;
	std::vector<std::vector<double>> timing; // till getting pos, img tstamp, till img conversion, mainloop till capture, mainloop till detect, mainloop till pnp, mainloop till cmd gen
	
	std::vector<std::vector<double>> angular_vel_avg;
	std::vector<std::vector<double>> angular_vel;
	std::vector<double> rot_angle;

	std::vector<std::vector<double>> quat_vec;
	std::vector<cv::Mat> target_tvecs_vec;
	// angular velocity to be added

	// for realtime tracking pnp, store the amount of keypoint detected
	std::vector<cv::Point3f> target_obj_4PnP; // also the reference
	std::vector<cv::Point2f> keyPnts_img_4PnP;
	//unsigned int num_active_kpt = pnt_num;

	// For manual target initilization
	Manual_ROI myPnt_ROI;

	// toolpath (defined in target frame)
	MyToolpath myToolpath = MyToolpath();

	// deformed waypoint
	cv::Mat deformed_waypnt;
	cv::Mat last_deformed_waypnt = cv::Mat::zeros(3, 1, CV_64FC1);

	MyCam();
	~MyCam();

	// Cam related
	int Configure(FlyCapture2::PGRGuid guid, bool Timing_F);
	int StartStreaming();
	int CaptureImage(MyPrt* myPrinter, bool Timing_F, bool INIT);
	int CaptureImage1_gettingPos(MyPrt* myPrinter, bool Timing_F, bool INIT, bool DEFORM, myPCA* deformObj);
	int CaptureImage1_noGettingPos(bool Timing_F, bool INIT, bool DEFORM, myPCA* deformObj);
	int CaptureImage2_onlyCapture(bool Timing_F);
	int CaptureImage3_postCapturing(bool Timing_F);

	int StopStream();
	int CamCalibration(bool Timing_F, bool Read_file);

	// Cam-Prt calibration
	int HandEyeCalib(MyPrt* myPrinter, std::vector<std::vector<DOUBLE>>* prt_traj, bool read_file, bool verify_f);

	// Target related
	int initKeyPnt(bool readfile_f, std::string file_name);
	int detectKeyPnt(bool DEBUG);
	int poseEstimation(MyPrt* myPrinter, bool DEBUG, bool Prediction, bool INIT);
	int dummy_tipLocating();
	int tipLocating2D(bool read_tip);
	int generateCmd(MyPrt* myPrinter, bool wait_flag, bool debug_f, bool Prediction, bool thread_enable, double z_offst, bool DEFORM, myPCA* deformObj, bool LOG_DATA); //, cv::Mat target_pnt
};
#endif



