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

#define prescale0 3
#define begin {
#define end }
#define t0 50 
#define steps_per_inch 48/1.25  
#define D 36 //length between motors in inches.
#define max 3
     
void initialize(void);

void draw_line(void);

void calculate_da(void);
void calculate_db(void); 
void calculate_dy(void);
  
void btick_once_R(void);
void ftick_once_R(void);
void btick_once_L(void);
void ftick_once_L(void);
void steptestL(void);
void steptestR(void); 
void manual_move(void);
void calculate_change();


unsigned int time0, stepcountR, stepcountL, condition;
unsigned char reload, stepR, stepL, flag1, flag2, flag3;                                                                            
float init_y, init_x, current_x, current_y, current_a, current_b, da, db, 
length_x, length_y, dx, dy;
float slope, y_intercept, x_dest, y_dest;
int da_funct;

flash float len_x[max] = {6, 6, -12};
flash float len_y[max] = {-12, 12, 0};
flash float d_x[max] = {.1, .1, -.1};
flash float d_y[max] = {-6/3*.1, 6/3*.1, 0};

char outstring[5];

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;            
 flag1 = 1;
 flag2 = 1;
 flag3 = 0;
 length_x =0;
 length_y =0;	
 da_funct=-1;
 
//intial conditions of string
current_a = D;
current_b = D;
current_y = 0.866 * D; 
current_x = D/2;
init_y = current_y; 
init_x = current_x; 
  //crank up the ISRs
  #asm
  	sei
  #endasm 
end

void main(void)
begin
   initialize();
   calculate_da();
   calculate_db();
       
   while(1)                                               
   begin
    if(time0 == 0)
    begin
    	time0 = t0;
    	manual_move();
		draw_line();
		PORTB = PORTC; 
	end
  end
end    	

void steptestL(void)
begin
  	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 steptestR(void)
begin
	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 btick_once_L(void)
begin
   	if (stepL > 0)
   	begin					
   		stepL--;
   	end
   	else stepL = 3;	 
   	stepcountL++;   
   	steptestL();
		 		
end


void ftick_once_L(void)
begin
   	if (stepL < 3)
   	begin					
   		stepL++;
   	end
   	else stepL = 0;
   	
   	stepcountL++;
   	steptestL();
end

void btick_once_R(void)
begin
	if (stepR < 3)
	begin					
		stepR++;
	end
	else stepR = 0;  
      
   	stepcountR++;
	steptestR();
end 

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

	stepcountR++;
	steptestR();
end 


void calculate_da(void)
begin
	da = ((current_x / current_a) * dx) + ((current_y / current_a) * dy);
end

void calculate_db(void)
begin
	db = (((current_x - D)/ (current_b)) * dx) + ((current_y / current_b) * dy);
end               

void calculate_change(void)
begin
	y_dest = init_y + length_y;
	x_dest = init_x + length_x;
end                              

void draw_line(void)
begin     		  
    calculate_change();
	if(dx>0)
		if (dy>0)
		    condition = (current_y  < y_dest) || (current_x  < x_dest);	  		
		else
			condition = (current_y  > y_dest) || (current_x  < x_dest);	  					
	else
		if (dy>0)
			condition = (current_y  < y_dest) || (current_x  > x_dest);	  		
		else
			condition = (current_y  > y_dest) || (current_x  > x_dest);	  		
			
			
	
	if(condition==1 && flag3 == 1)
	begin  
	  
		if(stepcountR < fabs(steps_per_inch*db*flag1))
			if(db<0) 
				btick_once_R();
			else
				ftick_once_R();
	    else
	    begin
	    	stepcountR = 0;
	    	flag1=0;
	    end	   
	    
		if(stepcountL < fabs(steps_per_inch*da*flag2))
			if(da<0)
				btick_once_L();
			else
				ftick_once_L();	
	  	else
	  	begin
	  		stepcountL = 0;
	  		flag2=0;
	    end
	    
		if(stepcountL==0 && stepcountR==0) 
		begin
			current_y = current_y + dy;
			current_x = current_x + dx;
			current_a = current_a + da;   
			current_b = current_b + db;
        	calculate_da();
        	calculate_db();
			flag1 = 1;
			flag2 = 1; 
			//delay_ms(1000);		
		end
						
	end 
	
	else if(flag3==1)
	begin
		da_funct++; 
		//printf("\n\rdafunct: %i", da_funct);
		//printf("\n\rflag3: %i", flag3);
		if(da_funct>=max)
			flag3=0;
		else
		begin
			length_x=len_x[da_funct];
	   		length_y=len_y[da_funct];
	   		dx = d_x[da_funct];
	   		dy = d_y[da_funct];         
	   		init_y = current_y;
	   		init_x = current_x;
      		calculate_da();
        	calculate_db(); 
	    	flag1 = 1;
 	    	flag2 = 1; 	     
 	    end
	end    



end

void manual_move(void)
begin
	if ((~PIND&0xF0) == 16)
  			begin
  				if (stepR > 0)
  		 			stepR--;
  				else stepR = 3;
  			end
  	 		else if ((~PIND&0xF0) == 32)
  	 		begin
  		 		if (stepR < 3)
				 		stepR++;
		 	 		else stepR = 0;  
		 	 	end	
	 		
  			if ((~PIND&0x0F) == 1)
  			begin
  				if (stepL > 0)
  		 			stepL--;
  				else stepL = 3;
  			end
  	 		else if ((~PIND&0x0F) == 2)
  	 		begin
  		 		if (stepL < 3)
				 		stepL++;
		 	 		else stepL = 0; 	   
	 		end                     
	 		
	 		else if(~PIND == 128 && da_funct < 4)
	 		begin
	 			da_funct=0;
	 			flag3 = 1;
	 			stepR=0;  
				stepL=0;
  				stepcountR=0;
  				stepcountL=0; 
				flag1 = 1;
 				flag2 = 1;
				length_x=len_x[da_funct];
				length_y=len_y[da_funct];
				dx = d_x[da_funct];
				dy = d_y[da_funct];         
				init_y = current_y;
				init_x = current_x;
                calculate_da();
                calculate_db(); 
	 		end
     		
	 		steptestL();
	 		steptestR();
	 			
end