/*
  kinematics_polar_coaster.cpp - Implements simple inverse kinematics for Grbl_ESP32
  Part of Grbl_ESP32

  Copyright (c) 2019 Barton Dring @buildlog


  Grbl is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.

  Grbl is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with Grbl.  If not, see <http://www.gnu.org/licenses/>.

	Inverse kinematics determine the joint parameters required to get to a position
	in 3D space. Grbl will still work as 3 axes of steps, but these steps could
	represent angles, etc instead of linear units.

	Unless forward kinematics are applied to the reporting, Grbl will report raw joint
	values instead of the normal Cartesian positions

	How it works...

	If you tell it to go to X10 Y10 Z10 in Cartesian space, for example, the equations
  will convert those values to the required joint values. In the case of a polar machine, X represents the radius,
	Y represents the polar degrees and Z would be unchanged.

	In most cases, a straight line in Cartesian space could cause a curve in the new system.
	To fix this, the line is broken into very small segments and each segment is converted
	to the new space. While each segment is also distorted, the amount is so small it cannot be seen.

	This segmentation is how normal Grbl draws arcs.

	Feed Rate

	Feed rate is given in steps/time. Due to the new coordinate units and non linearity issues, the
	feed rate may need to be adjusted. The ratio of the step distances in the original coordinate system
	determined and applied to the feed rate.

	TODO:
		Add y offset, for completeness
		Add ZERO_NON_HOMED_AXES option


*/
#include "grbl.h"
#ifdef CPU_MAP_POLAR_COASTER
#ifdef USE_KINEMATICS

static float last_angle = 0;
static float last_radius = 0;

// this get called before homing 
// return false to complete normal home
// return true to exit normal homing
bool kinematics_pre_homing(uint8_t cycle_mask) {
	// cycle mask not used for polar coaster	
	
	// zero the axes that are not homed
	sys_position[Y_AXIS] = 0.0f; 
	sys_position[Z_AXIS] = SERVO_Z_RANGE_MAX * settings.steps_per_mm[Z_AXIS]; // Move pen up.

	return false; // finish normal homing cycle
}

void kinematics_post_homing() {
	// sync the X axis (do not need sync but make it for the fail safe)
	last_radius = sys_position[X_AXIS];
	// reset the internal angle value
	last_angle = 0;
}

/*
 Apply inverse kinematics for a polar system

 float target: 					The desired target location in machine space
 plan_line_data_t *pl_data:		Plan information like feed rate, etc
 float *position:				The previous "from" location of the move

 Note: It is assumed only the radius axis (X) is homed and only X and Z have offsets


*/
void inverse_kinematics(float *target, plan_line_data_t *pl_data, float *position)
{
	//static float last_angle = 0;
	//static float last_radius = 0;

	float dx, dy, dz; // distances in each cartesian axis
	float p_dx, p_dy, p_dz; // distances in each polar axis

	float dist, polar_dist; // the distances in both systems...used to determine feed rate

	uint32_t segment_count;  // number of segments the move will be broken in to.
	float seg_target[N_AXIS]; // The target of the current segment

	float polar[N_AXIS]; // target location in polar coordinates

	float x_offset = gc_state.coord_system[X_AXIS]+gc_state.coord_offset[X_AXIS]; // offset from machine coordinate system
	float z_offset = gc_state.coord_system[Z_AXIS]+gc_state.coord_offset[Z_AXIS]; // offset from machine coordinate system

	//grbl_sendf(CLIENT_SERIAL, "Position: %4.2f %4.2f %4.2f \r\n", position[X_AXIS] - x_offset, position[Y_AXIS], position[Z_AXIS]);
	//grbl_sendf(CLIENT_SERIAL, "Target: %4.2f %4.2f %4.2f \r\n", target[X_AXIS] - x_offset, target[Y_AXIS], target[Z_AXIS]);

	// calculate cartesian move distance for each axis
	dx = target[X_AXIS] - position[X_AXIS];
	dy = target[Y_AXIS] - position[Y_AXIS];
	dz = target[Z_AXIS] - position[Z_AXIS];

	// calculate the total X,Y axis move distance
	// Z axis is the same in both coord systems, so it is ignored
	dist = sqrt( (dx * dx) + (dy * dy) + (dz * dz));

	if (pl_data->condition & PL_COND_FLAG_RAPID_MOTION) {
		segment_count = 1;    // rapid G0 motion is not used to draw, so skip the segmentation
	} else {
		segment_count = ceil(dist / SEGMENT_LENGTH);    // determine the number of segments we need	... round up so there is at least 1
	}

	dist /= segment_count;  // segment distance


	for(uint32_t segment = 1; segment <= segment_count; segment++) {
		// determine this segment's target
		seg_target[X_AXIS] = position[X_AXIS] + (dx / float(segment_count) * segment) - x_offset;
		seg_target[Y_AXIS] = position[Y_AXIS] + (dy / float(segment_count) * segment);
		seg_target[Z_AXIS] = position[Z_AXIS] + (dz / float(segment_count) * segment) - z_offset;

		calc_polar(seg_target, polar, last_angle);
		
		// begin determining new feed rate
		
		// calculate move distance for each axis
		p_dx = polar[RADIUS_AXIS] - last_radius;
		p_dy = polar[POLAR_AXIS] - last_angle;
		p_dz = dz;

		polar_dist = sqrt( (p_dx * p_dx) + (p_dy * p_dy) +(p_dz * p_dz)); // calculate the total move distance

		float polar_rate_multiply = 1.0;  // fail safe rate
		if (polar_dist == 0 || dist == 0) { // prevent 0 feed rate and division by 0
			polar_rate_multiply = 1.0;    // default to same feed rate
		} else {
			// calc a feed rate multiplier
			polar_rate_multiply = polar_dist / dist;
			if (polar_rate_multiply < 0.5) { // prevent much slower speed
				polar_rate_multiply = 0.5;
			}
		}

		pl_data->feed_rate *= polar_rate_multiply;  // apply the distance ratio between coord systems
		
		// end determining new feed rate 

		polar[RADIUS_AXIS] += x_offset;
		polar[Z_AXIS] += z_offset;

		last_radius = polar[RADIUS_AXIS];
		last_angle = polar[POLAR_AXIS];
		
		mc_line(polar, pl_data);
	}
	
	// TO DO don't need a feedrate for rapids
}


/*
Forward kinematics converts position back to the original cartesian system. It is
typically used for reporting

For example, on a polar machine, you tell it to go to a place like X10Y10. It 
converts to a radius and angle using inverse kinematics. The machine posiiton is now 
in those units X14.14 (radius) and Y45 (degrees). If you want to report those units as 
X10,Y10, you would use forward kinematics

position = the current machine position
converted = position with forward kinematics applied.

*/
void forward_kinematics(float *position)
{
	float original_position[N_AXIS]; // temporary storage of original
	float print_position[N_AXIS];
	int32_t current_position[N_AXIS]; // Copy current state of the system position variable
	
	memcpy(current_position,sys_position,sizeof(sys_position));
	system_convert_array_steps_to_mpos(print_position,current_position);
	
	original_position[X_AXIS] = print_position[X_AXIS] - gc_state.coord_system[X_AXIS]+gc_state.coord_offset[X_AXIS];
	original_position[Y_AXIS] = print_position[Y_AXIS] - gc_state.coord_system[Y_AXIS]+gc_state.coord_offset[Y_AXIS];
	original_position[Z_AXIS] = print_position[Z_AXIS] - gc_state.coord_system[Z_AXIS]+gc_state.coord_offset[Z_AXIS];
	
	position[X_AXIS] = cos(radians(original_position[Y_AXIS])) * original_position[X_AXIS] * -1;
	position[Y_AXIS] = sin(radians(original_position[Y_AXIS])) * original_position[X_AXIS];
	position[Z_AXIS] = original_position[Z_AXIS];  // unchanged
}

// helper functions

/*******************************************
* Calculate polar values from Cartesian values
* 	float target_xyz:	An array of target axis positions in Cartesian (xyz) space
*		float polar: 			An array to return the polar values
*		float last_angle:	The polar angle of the "from" point.
*
*   Angle calculated is 0 to 360, but you don't want a line to go from 350 to 10. This would
*   be a long line backwards. You want it to go from 350 to 370. The same is true going the other way.
*
*   This means the angle could accumulate to very high positive or negative values over the coarse of
*   a long job.
*
*/
void calc_polar(float *target_xyz, float *polar, float last_angle)
{
	float delta_ang; // the difference from the last and next angle

	polar[RADIUS_AXIS] = hypot_f(target_xyz[X_AXIS], target_xyz[Y_AXIS]);
	
	if (polar[RADIUS_AXIS] == 0) {
		polar[POLAR_AXIS] = last_angle; // don't care about angle at center
	}
	else {
		polar[POLAR_AXIS] = atan2(target_xyz[Y_AXIS], target_xyz[X_AXIS]) * 180.0 / M_PI;

		// no negative angles...we want the absolute angle not -90, use 270		
		polar[POLAR_AXIS] = abs_angle(polar[POLAR_AXIS]);
	}	

	polar[Z_AXIS] = target_xyz[Z_AXIS]; // Z is unchanged

	delta_ang = polar[POLAR_AXIS] -  abs_angle(last_angle);

	// if the delta is above 180 degrees it means we are crossing the 0 degree line
	if ( fabs(delta_ang) <= 180.0) {
		polar[POLAR_AXIS] = last_angle + delta_ang;
	} else {	
		if (delta_ang > 0.0)  { // crossing zero counter clockwise
			polar[POLAR_AXIS] = last_angle - (360.0 - delta_ang);
		} else {
			polar[POLAR_AXIS]  = last_angle + delta_ang + 360.0;
		}
	}
}

// Return a 0-360 angle ... fix above 360 and below zero
float abs_angle(float ang) {
	ang = fmod(ang, 360.0);  // 0-360 or 0 to -360

	if (ang < 0.0) {
		ang = 360.0 + ang;
	}
	return ang;
}

#endif

// Polar coaster has macro buttons, this handles those button pushes.
void user_defined_macro(uint8_t index)
{
	//grbl_sendf(CLIENT_SERIAL, "[MSG: Macro #%d]\r\n", index);
	switch (index) {
		#ifdef MACRO_BUTTON_0_PIN
		case CONTROL_PIN_INDEX_MACRO_0:
			inputBuffer.push("$H\r"); // home machine
		break;
		#endif
		#ifdef MACRO_BUTTON_1_PIN
		case CONTROL_PIN_INDEX_MACRO_1:
			inputBuffer.push("[ESP220]/1.nc\r"); // run SD card file 1.nc
		break;
		#endif
		#ifdef MACRO_BUTTON_2_PIN
		case CONTROL_PIN_INDEX_MACRO_2:
			inputBuffer.push("[ESP220]/2.nc\r"); // run SD card file 2.nc
		break;
		#endif
		#ifdef MACRO_BUTTON_3_PIN
		case CONTROL_PIN_INDEX_MACRO_3:
		break;
		#endif
		default:
		break;
	}
}

// handle the M30 command
void user_m30() {
	inputBuffer.push("$H\r");
}

#endif