// Identical to servoabs.c, but includes fluid movement routines. Uses an extra timer to accomlish this.
// #device HIGH_INTS=TRUE required at the top of the main file. Without interrupt priority on timer3, the servos
// jitter everytime timer2's isr interrupts timer3's isr

// With a 20Mhz crystal, and T1_DIV_8, T1 interupts every (4*8*(2^16-53660))/20000000 = 19ms < 20ms
#define TIMER_VAL	53660

// Specifics of the servos
#define SHORT_TIME	0.0010
#define LONG_TIME	0.0020
#define SCALE		(LONG_TIME-SHORT_TIME)/255.0

// Angles of servos. timerangle is the time duration of timer3 corresponding to the pulsewidth required for the angle.
int8 angle[] = {0,0,0,0,0,0,0,0,0};
int16 timerangle[] = {0,0,0,0,0,0,0,0,0};

// move angle[i] by move[i] every interrupt of timer2
signed int8 move[] = {0,0,0,0,0,0,0,0,0};

// Pin description of the servos. number of servos. Maximum of 9. Might be able to squeeze a few more in.
int16 servo[] = {0,0,0,0,0,0,0,0,0};
int8 num = 0;

// Keeps track of the servo we are currently dealing with.
int8 cur;

// Timer2 can't make intervals long enough
int8 timer2extend;

// Turn an angle into a timer length. Angle 0 is full left, angle 255 is full right, (I think).
int16 calc_tangle(int8 a) {
	return 0xFFFF- (int16)((a*SCALE + SHORT_TIME)*1250000);
}

// Input the pin descriptions of the servos, eg. int16 servos[] = {PIN_B6, PIN_C5, PIN_B2}; init_servoext(servos, 3);
void init_servoext(int16 s[], int8 n) {
	int8 i;

	num = n;
	cur = 0;

	for (i = 0; i < n; i++) {
		if (s[i] != 0) {
			servo[i] = s[i];
		}
	}

	for (i = 0; i < num; i++) {
		angle[i] = 127;
		timerangle[i] = calc_tangle(angle[i]);
	}

	setup_timer_2(T2_DIV_BY_16, 125, 16);
	set_timer1(0);
	timer2extend = 0;

	setup_timer_1(T1_DIV_BY_8 | T1_INTERNAL);
	set_timer1(TIMER_VAL);
	
	setup_timer_3(T3_DIV_BY_4 | T3_INTERNAL);

	enable_interrupts(INT_TIMER1);
	enable_interrupts(INT_TIMER2);
	disable_interrupts(INT_TIMER3);
	enable_interrupts(GLOBAL);

}

// Set the angle of servo i. Angle 0 is full left, angle 255 is full right
void set_angle(int8 ang, int8 i) {
	angle[i] = ang;
	timerangle[i] = calc_tangle(ang);
}

int8 get_angle(int8 i) {
	return angle[i];
}

// Set the movement of servo i
void set_move(signed int8 m, int8 i) {
	move[i] = m;
}

signed int8 get_move(int8 i) {
	return move[i];
}

// Equivalent to set_move(0, i)
void stop(int8 i) {
	move[i] = 0;
}

#int_timer1
void isr_timer1() {
	cur = 0;
	output_high(servo[cur]);
	set_timer3(timerangle[cur]);
	enable_interrupts(int_timer3);

	clear_interrupt(INT_TIMER1);
	set_timer1(TIMER_VAL);
}

#int_timer3 high
void isr_timer3() {
	output_low(servo[cur]);

	cur++;
	if (cur == num) {
		disable_interrupts(INT_TIMER3);
		clear_interrupt(INT_TIMER3);
	} else {
		output_high(servo[cur]);
		clear_interrupt(INT_TIMER3);
		set_timer3(timerangle[cur]);
	}

}

#int_timer2
void isr_timer2() {
	timer2extend ++;
	if (timer2extend == 4) {
		int8 i;
		output_low(PIN_B6);
		for (i = 0; i < num; i ++) {
			if (move[i] > 0) {
				if (255 - angle[i] >= move[i]) {
					set_angle(angle[i] + move[i], i);
				} else {
					set_angle(255, i);
				}
			}
			if (move[i] < 0) {
				if (angle[i] <= move[i]*(-1)) {
					set_angle(0, i);
				} else {
					set_angle(angle[i] + move[i], i);
				}
			}
		}
		timer2extend = 0;
		output_high(PIN_B6);
	}

	clear_interrupt(INT_TIMER2);
	set_timer2(0);
}