#include "node_yaw_rot.h"
#include <stdlib.h>

static void rotate_yaw(void *state, const double* params, const double *inputs, double *outputs) {
	// Psuedo-Nonlinear Extension for determining local x/y position based on yaw angle
	// provided by Matt Rich
	//
	// local x/y/z is the moving frame of reference on the quad that we are transforming so we can assume yaw angle is 0 (well enough)
	// 		for the autonomous position controllers
	//
	// camera given x/y/z is the inertia frame of reference (the global coordinates)
	//
	// |local x|	|cos(yaw angle)  -sin(yaw angle)  0| |camera given x|
	// |local y|  = |sin(yaw angle)   cos(yaw angle)  0| |camera given y|
	// |local z|	|       0               0         1| |camera given z|

    outputs[ROT_OUT_X] =
        inputs[ROT_CUR_X] * cos(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * -sin(inputs[ROT_YAW]);

	outputs[ROT_OUT_Y] =
			inputs[ROT_CUR_X] * sin(inputs[ROT_YAW]) + inputs[ROT_CUR_Y] * cos(inputs[ROT_YAW]);
}
static void reset(void *state) {}

static const char* const in_names[3] = {"Current Yaw", "X Position", "Y Position"};
static const char* const out_names[2] = {"Rotated X", "Rotated Y"};
static const char* const param_names[1] = {"Error if you see this"};
const struct graph_node_type node_yaw_rot_type = {
        .input_names = in_names,
        .output_names = out_names,
        .param_names = param_names,
        .n_inputs = 3,
        .n_outputs = 2,
        .n_params = 0,
        .execute = rotate_yaw,
        .reset = reset,
        .state_size = 0,
        .type_id = BLOCK_YAW_ROT
};