#include "mode_openloop.h"

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


void mode_openloop_init(app_state_t *app, mode_openloop_state_t *state) {
	state->time = 0;
	state->is_continuous = 0;
	state->velocity = 0.0;
	state->prev_mc = 0;
}

void mode_openloop(app_state_t *app, mode_openloop_state_t *state) {
	ctre_feed_enable(app->switch_state > 0);

	if(app->switch_state == 1) {
		if(!state->is_continuous) memecoder_zero();
		state->is_continuous = 1;
	} else {
		state->is_continuous = 0;
	}

    if(state->time % 20 == 0) {
        int mc = memecoder_get();
//    	printf("mc: %d, is_continuous: %d\r\n", mc, state->is_continuous);
        if(state->is_continuous) {
        	ctre_percent_out(app->device, (double)mc / 128.0);
//	    	ctre_position_out(app->device, mc * 1024 / 42);
        } else {
			int delta = mc - state->prev_mc;
			int delta_exp = delta * delta;
			if(delta < 0) delta_exp *= -1;
			state->velocity += (double)delta_exp * .01;
			ctre_percent_out(app->device, state->velocity);
//			printf("velocity: %lf\r\n", state->velocity);
        }
		state->prev_mc = mc;
		state->velocity *= 0.90;
    }

	state->time++;
}