#include "mode_position2.h"

#include "ctre.h"
#include "memecoder.h"



void mode_position2_init(app_state_t *app, mode_position2_state_t *state) {
	state->time = 0;
	state->zeroed = 0;
	state->position = 0;
	state->position2 = 0;
}

void mode_position2(app_state_t *app, mode_position2_state_t *state) {
	ctre_feed_enable(app->switch_state > 0);

	printf("Pos: %d, %d\r\n", state->position, state->position2);
	if(app->switch_state > 0) {
		if(!state->zeroed) {
			if(app->switch_state == 1)
				memecoder_set(state->position * 42 / 2048 / 5);
			else
				memecoder_set(state->position2 * 42 / 2048 / 5);
		}
		state->zeroed = 1;
	} else {
		state->zeroed = 0;
	}

    if(state->time % 20 == 0) {
        int mc = memecoder_get();
    	int cmd_pos = mc * 5 * 2048 / 42;
    	if(app->switch_state == 1) {
    		state->position = (state->position * 9 + cmd_pos) / 10;
    		ctre_position_out(10, state->position);
    	}
    	if(app->switch_state == 2) {
    		state->position2 = (state->position2 * 9 + cmd_pos) / 10;
    		ctre_position_out(30, state->position2);
    	}
    }

	state->time++;
}