#include "mode_position.h"

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


int dicks;
int balls;

void mode_position_init(app_state_t *app, mode_position_state_t *state) {
	state->time = 0;
	state->zeroed = 0;
	state->position = 0;
	dicks = 1;
	balls = 0;
}

void mode_position(app_state_t *app, mode_position_state_t *state) {
	ctre_feed_enable(app->switch_state == 2);

	printf("Pos: %d\r\n", state->position);
	if(app->switch_state == 1) {
		if(!state->zeroed) {
			state->position = 0;
			memecoder_zero();

			// Set sensor position to 0
			ctre_set_param(app->device, 331, 0, 0, 0);
		}
		state->zeroed = 1;
	} else {
		state->zeroed = 0;
	}

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

    if(dicks % 2000 == 0) {
    	balls = !balls;
    	dicks = 1;

    	if(balls) {
//	    	state->position = -133120;
//    		state->position = 95320;
    		state->position = 350000;
    	} else {
    		state->position = 0;
    	}
    }

    dicks++;
	state->time++;
}