#include <90s8515.h>
#include <Stdio.h>
#include <Stdlib.h>
#include <delay.h>

#define prescale0 3
#define begin {
#define end }
#define t0 25 
#define maxstep 50  
#define dx 0.1; 
#define dy 0.1; 
#define D 39 //length between motors in inches.
     
void initialize(void);
void draw_v(void);
void draw_v2(void);
void calculate_da_v(void);
void calculate_db_v(void);    
void btick_once_R(void);
void ftick_once_R(void);
void btick_once_L(void);
void ftick_once_L(void);


unsigned int time0, stepcountR, stepcountL, num_rotations;
unsigned char reload, stepR, stepL, length;                                                                            
float current_x, current_y, current_a, current_b, da, db;

interrupt [TIM0_OVF] void timer0_overflow(void)
begin   
    
  //reload to force 1 mSec overflow
  TCNT0=reload;
  
  //Decrement the times if they are not already zero
  if (time0>0)	--time0;
  
end  

//********************************************************** 
//Set it all up
void initialize(void)
begin
  
  //set up the port
  DDRC=0xFF; 	//make port C output
  PORTC = 0;
  
  DDRB=0xFF;
  PORTB = 0;              
  
  //serial setop for debugging using printf, etc.     
  UCR = 0x10 + 0x08 ;
  UBRR = 25 ;
  
  //set up timer 0     
  //62.5 x (64x.25) microSec = 1.0 mSec, so prescale 64, and count 62 times.
  reload=256-62; 		//value for 1 Msec  
  TCNT0=reload;	 		//preload timer 1 so that is interrupts after 1 mSec.
  TCCR0=prescale0;		//prescalar to 64
  TIMSK=2;	   		//turn on timer 0 overflow ISR  
  
  //init the task timer
  time0=t0;
  stepR=0;  
  stepL=0;
  stepcountR=0;
  stepcountL=0;  
 
 num_rotations = 6;
 length = 6;
 
//intial conditions of string
current_a = D;
current_b = D;
current_y = 0.866 * D; 
 
  //crank up the ISRs
  #asm
  	sei
  #endasm 
end

void main(void)
begin
  initialize();           
	while(1)
  begin
    if(time0 == 0)
    begin
    	time0 = t0;
		draw_v2();
		PORTB = PORTC;
	end
  end
end    	

void btick_once_L(void)
begin
   	if (stepL > 0)
   	begin					
   		stepL--;
   	end
   	else stepL = 3;	 
   	stepcountL++;   
   	if (stepL == 0) PORTC = 16+32+(PORTC&0x0F);
	if (stepL == 1) PORTC = 32+64+(PORTC&0x0F);
	if (stepL == 2) PORTC = 64+128+(PORTC&0x0F);
	if (stepL == 3) PORTC = 128+16+(PORTC&0x0F);
		 		
end


void ftick_once_L(void)
begin
   	if (stepL < 3)
   	begin					
   		stepL++;
   	end
   	else stepL = 0;
   	
   	stepcountL++;
   	if (stepL == 0) PORTC = 16+32+(PORTC&0x0F);
	if (stepL == 1) PORTC = 32+64+(PORTC&0x0F);
	if (stepL == 2) PORTC = 64+128+(PORTC&0x0F);
	if (stepL == 3) PORTC = 128+16+(PORTC&0x0F);	 		
end

void btick_once_R(void)
begin
	if (stepR < 3)
	begin					
		stepR++;
	end
	else stepR = 0;  
      
   	stepcountR++;
	if (stepR == 0) PORTC = 1+2+(PORTC&0xF0);
	if (stepR == 1) PORTC = 2+4+(PORTC&0xF0);
	if (stepR == 2) PORTC = 4+8+(PORTC&0xF0);
	if (stepR == 3) PORTC = 8+1+(PORTC&0xF0);
end 

void ftick_once_R(void)
begin
	if (stepR > 0)
	begin					
		stepR--;
	end
	else stepR = 3;  

	stepcountR++;
	if (stepR == 0) PORTC = 1+2+(PORTC&0xF0);
	if (stepR == 1) PORTC = 2+4+(PORTC&0xF0);
	if (stepR == 2) PORTC = 4+8+(PORTC&0xF0);
	if (stepR == 3) PORTC = 8+1+(PORTC&0xF0);
end 

void draw_v(void)
begin
  	if(stepcountR < maxstep*num_rotations)
  	begin
   		ftick_once_R();
   	end  
	if(stepcountL < maxstep*num_rotations)
		ftick_once_L();
end

void calculate_da_v(void)
begin
	da = (current_y / current_a) * dy;
end

void calculate_db_v(void)
begin
	db = (current_y / current_b) * dy;
end               

void draw_v2(void)
begin     			  		
	if(current_y > 0.866 * D - length)
	begin         
		calculate_da_v();
		calculate_db_v();
	
		if(stepcountR < 50*da) 
			ftick_once_R();
	
		if(stepcountL < 50*da)
			ftick_once_L();
	
		current_y = current_y - dy;
	end
end