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

static int pwm_min = 100000;
static int pwm_max = 200000;

static int pwm_clamp(int val) {
	if (val < pwm_min) {val = pwm_min;}
	if (val > pwm_max) {val = pwm_max;}
	return val;
}

static void mixer_computation(void *state, const double* params, const double *inputs, double *outputs) {
	int pwm0 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] - inputs[MIXER_ROLL] - inputs[MIXER_YAW];
	int pwm1 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] - inputs[MIXER_ROLL] + inputs[MIXER_YAW];
	int pwm2 = inputs[MIXER_THROTTLE] - inputs[MIXER_PITCH] + inputs[MIXER_ROLL] + inputs[MIXER_YAW];
	int pwm3 = inputs[MIXER_THROTTLE] + inputs[MIXER_PITCH] + inputs[MIXER_ROLL] - inputs[MIXER_YAW];
	outputs[MIXER_PWM0] = pwm_clamp(pwm0);
	outputs[MIXER_PWM1] = pwm_clamp(pwm1);
	outputs[MIXER_PWM2] = pwm_clamp(pwm2);
	outputs[MIXER_PWM3] = pwm_clamp(pwm3);
}

static void reset_mixer(void *state) {}

static const char* const in_names[4] = {"Throttle", "Pitch", "Roll", "Yaw"};
static const char* const out_names[4] = {"PWM 0", "PWM 1", "PWM 2", "PWM 3"};
static const char* const param_names[1] = {"You shouldnt see this"};
const struct graph_node_type node_mixer_type = {
        .input_names = in_names,
        .output_names = out_names,
        .param_names = param_names,
        .n_inputs = 4,
        .n_outputs = 4,
        .n_params = 0,
        .execute = mixer_computation,
        .reset = reset_mixer,
		.state_size = 0,
		.type_id = BLOCK_MIXER
};