#include <avr/io.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include "sio.c"
#include "sio.h"

#define c_kinetic_pos 0x08 /* static friction */
#define c_kinetic_neg 0x08 /* make the value positive */
#define c_static_pos 0x0a /* kinetic friction */
#define c_static_neg 0x0a /* make the value positive */
#define c_max 255
#define c_min 255 /* make the value positive */

int moving = 0; /* assume it starts without moving */

int deadband(int c_wanted){ /* call this routine when updating */
	int c_pos;
	int c_neg;
	int c_adjusted;
	if(moving == 1){
		c_pos = c_kinetic_pos;
		c_neg = c_kinetic_neg;
	} else {
		c_pos = c_static_pos;
		c_neg = c_static_neg;
	}
	if(c_wanted == 0){ /* turn off the output */
		c_adjusted = 0;
	} else if(c_wanted > 0){ /* a positive output */
		c_adjusted = c_pos + (unsigned)(c_max - c_pos) * c_wanted / c_max;
	if(c_adjusted > c_max) 
		c_adjusted = c_max;
	} else { /* the output must be negative */
		c_adjusted = -c_neg -(unsigned)(c_min - c_neg) * -c_wanted / c_min;
	if(c_adjusted < -c_min) 
		c_adjusted = -c_min;
	}
	return c_adjusted;
}
void PWM_init(){ /* call this routine once when the program starts */
	DDRD |= (1 << PD5); /* set PWM outputs */
	DDRC |= (1 << PC0) | (1 << PC1);
	/* set motor direction outputs on port C*/
	//using OCR1
	TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
	// turn on both PWM outputs on counter 1
	TCCR1B = _BV(CS11) ; // set the internal clock to /8
}
void PWM_update(int value){ /* to update the PWM output */
	if(value > 255) value = 255;
	if(value < 0) value = 0;
	outint(value); outln(" = PWM");
	OCR1A = value; // duty cycle
}
	int v_output(int v_adjusted){ /* call from the interrupt loop */
	int RefSignal; // the value to be returned
	if(v_adjusted >= 0){
		/* set the direction bits to CW on, CCW off */
		PORTC = (PINC & 0xFC) | 0x02; /* bit 1 on, 0 off */
	if(v_adjusted > 255){ /* clip output over maximum */
		RefSignal = 255;
	} else {
		RefSignal = v_adjusted;
	}
	} else { /* need to reverse output sign */
		/* set the direction bits to CW off, CCW on */
		PORTC = (PINC & 0xFC) | 0x01; /* bit 0 on, 1 off */
		if(v_adjusted < -255){ /* clip output below minimum */
			RefSignal = 255;
		} else {
		RefSignal = -v_adjusted; /* flip sign */
		}
	}
	return RefSignal;
}
int count = 0; // a count value to be output on port B

void IO_setup(){
	PWM_init();
}
int db_correct = 0; /* deadband correction is off by default */

void IO_update(){// This routine will run once per interrupt for updates
	if(db_correct == 0){
		PWM_update(v_output(count));
	} else {
		PWM_update(v_output(deadband(count)));
	}
}
void delay(int ticks){ // ticks are approximately 1ms
	volatile int i, j;
	for(i = 0; i < ticks; i++){
		for(j = 0; j < 1000; j++){}
	}
}
int main(){
	int c;
	sio_init();
	IO_setup();
	outln("");
	outln("--Deadband compensation program");
	for(;;){
		while((c = input()) == -1){} // wait for a keypress
		if(c == '+'){ // increment the output on port B
			if(++count > 255) count = 255;
			if(count == 1){ // it has just started moving
				moving = 0;
				IO_update();
				delay(200);
				moving = 1;
			}
			IO_update();
			outint(count); outln(" = count, incremented");
		} else if(c == '-'){
			if(--count < -255) count = -255;
			if(count == -1){ // it has just started moving
				moving = 0;
				IO_update();
				delay(200);
				moving = 1;
			}
			IO_update();
			outint(count); outln(" = count, decremented");
		} else if(c == 'd'){
			if(db_correct == 0){
				db_correct = 1;
				outln("deadband correction ON");
			} else {
				db_correct = 0;
				outln("deadband correction OFF");
			}
			IO_update();
		} else if(c == 's'){
			count = 0;
			moving = 0;
			IO_update();
			outint(count); outln(" = count, motor stopped");
		} else if(c == 'h'){
			outln(" +: increment value");
			outln(" -: decrement value");
			outln(" d: enable or disable (default) deadband comp.");
			outln(" s: stop the motor");
			outln(" q: quit");
		} else if(c == 'q'){
			count = 0;
			moving = 0;
			IO_update();
			outint(count); outln(" = count, Quitting!");
			break;
		}
	}
	sio_cleanup();
	
	return 1;
}