#ifndef MYTOOLPATH_H
#define MYTOOLPATH_H

#include <math.h>
#include <iostream>
#include <fstream>
#include <string>
#include "time.h"

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

#define PI 3.14159265

class MyToolpath {
public:
	// toolpath
	std::vector<cv::Mat> toolpath; // position info for toolpath
	std::vector<cv::Point3f> toolpath_pnt; // position info for toolpath
	std::vector<cv::Point2f> toolpath_pnt_imgpnt; 
	std::vector<double> toolpath_time; // time waypoint info for toolpath

	// Data logging
	std::vector<cv::Mat> toolpath_tip_traj;
	std::vector<cv::Mat> toolpath_desired_traj;
	std::vector<cv::Mat> toolpath_track_error;

	double toolpath_timestamp;
	double travel_time;
	double travel_dest_time;

	unsigned int toolpath_num_waypnt;
	unsigned int toolpath_idx = 0;

	double print_speed = 1.; // mm/s 
	

	// timer for speed tracking
	clock_t toolpath_tmr;

	MyToolpath();
	~MyToolpath();

	int init_toolpath_planar_circle(double usr_print_speed, double radius, unsigned int num_waypnt, int num_cycle);
	int init_toolpath_read_file(double usr_print_speed, std::string file_name);
};

#endif
