#include "../Header/myToolpath.h"

MyToolpath::MyToolpath() {
	
	std::cout << "Toolpath obj created" << std::endl;
}

MyToolpath::~MyToolpath() {
	std::cout << "Toolpath obj destroyed" << std::endl;
}


// Init toolpath with a ploanar circle (defined in target frame). 
// Output: print_speed, toolpath_num_waypnt, toolpath, toolpath_pnt, toolpath_time
int MyToolpath::init_toolpath_planar_circle(double usr_print_speed, double radius, unsigned int num_waypnt, int num_cycle) {
	print_speed = usr_print_speed;
	toolpath_num_waypnt = num_waypnt;

	// manual init of a circle
	cv::Mat temp_waypnt = cv::Mat::zeros(3, 1, CV_64FC1);
	
	double time = 0.;
	double p_inc = 0.;
	double theta = 0.; //rad
	double dtheta = 2. * num_cycle* PI / num_waypnt;

	// first waypoint
	temp_waypnt.at<double>(0, 0) = radius * cos(theta);
	temp_waypnt.at<double>(1, 0) = radius * sin(theta);
	
	toolpath.push_back(temp_waypnt); // for mat format

	cv::Point3f temp_waypnt_pnt(temp_waypnt);// for point3f format
	toolpath_pnt.push_back(temp_waypnt_pnt); 

	toolpath_time.push_back(time);
	theta = theta + dtheta;

	for (int i = 1; i < num_waypnt; i++) {
		// compute current waypoint
		temp_waypnt.at<double>(0, 0) = radius * cos(theta);
		temp_waypnt.at<double>(1, 0) = radius * sin(theta);

		toolpath.push_back(temp_waypnt.clone()); // for mat format

		cv::Point3f temp_waypnt_pnt(temp_waypnt);// for point3f format
		toolpath_pnt.push_back(temp_waypnt_pnt);

		// compute time taken to reach the waypoint
		p_inc = sqrt(pow(toolpath[i].at<double>(0, 0) - toolpath[i - 1].at<double>(0, 0), 2.) + pow(toolpath[i].at<double>(1, 0) - toolpath[i - 1].at<double>(1, 0), 2.));
		std::cout << "p_inc: " << p_inc << std::endl;
		time = time + p_inc * 1000 / print_speed; // ms
		std::cout << "time: " << time << std::endl;
		toolpath_time.push_back(time);

		// increase angle
		theta = theta + dtheta;
	}

	return 0;
}

int MyToolpath::init_toolpath_read_file(double usr_print_speed, std::string file_name) {
	print_speed = usr_print_speed;
	
	std::ifstream infile(file_name);
	cv::Mat temp_waypnt = cv::Mat::zeros(3, 1, CV_64FC1);

	unsigned int waypnt_counter = 0;
	float x, y, z;

	double time = 0.;
	double p_inc = 0.;

	// first waypoint
	infile >> x >> y >> z;
	waypnt_counter++;
	temp_waypnt.at<double>(0, 0) = (double)x;
	temp_waypnt.at<double>(1, 0) = (double)y;
	temp_waypnt.at<double>(2, 0) = (double)z;

	toolpath.push_back(temp_waypnt.clone()); // for mat format

	cv::Point3f temp_waypnt_pnt(temp_waypnt);// for point3f format
	toolpath_pnt.push_back(temp_waypnt_pnt);

	toolpath_time.push_back(time);

	while (infile >> x >> y >> z) {
		
		temp_waypnt.at<double>(0, 0) = (double)x;
		temp_waypnt.at<double>(1, 0) = (double)y;
		temp_waypnt.at<double>(2, 0) = (double)z;
		toolpath.push_back(temp_waypnt.clone()); // for mat format
		cv::Point3f temp_waypnt_pnt(temp_waypnt);// for point3f format
		toolpath_pnt.push_back(temp_waypnt_pnt);

		// compute time taken to reach the waypoint
		p_inc = sqrt(pow(toolpath[waypnt_counter].at<double>(0, 0) - toolpath[waypnt_counter - 1].at<double>(0, 0), 2.) + pow(toolpath[waypnt_counter].at<double>(1, 0) - toolpath[waypnt_counter - 1].at<double>(1, 0), 2.) + pow(toolpath[waypnt_counter].at<double>(2, 0) - toolpath[waypnt_counter - 1].at<double>(2, 0), 2.));
		//std::cout << "p_inc: " << p_inc << std::endl;
		time = time + p_inc * 1000 / print_speed; // ms
		//std::cout << "time: " << time << std::endl;
		//std::cout << x << " " << y << " " << z << std::endl;
		toolpath_time.push_back(time);

		waypnt_counter++;
	}
	std::cout << "printing time (s): " << time/1000 << std::endl;
	infile.close();

	toolpath_num_waypnt = waypnt_counter;

	return 0;
}