#include <Mega32.h>   
#include <stdio.h>
#include <stdlib.h> 
#include <math.h> 
#include <delay.h>   
#include "tv.h"            // stored all tv draw point/line functions in tv.h        
#define ONE_REV	254       // # revolutions in single rotation     
#define CENTERX 63		  // center point on x axis of tv screen
#define	CENTERY	50        // center point on y axis of tv screen 	 
#define LEFT 	12		  // arbitrary numbers designating turn directions
#define RIGHT   56
#define NOPREV	92    
#define OSCTURN	11                 
#define LCDwidth 16 //characters for lcd display width
                                     
#asm
    .equ __lcd_port=0x15                              // PORT C
#endasm
#include <lcd.h> // LCD driver routines

char lcd_buffer[17];    // LCD display buffer    
char osc1, osc2;        // dampen oscillations on motors           
char twoTurnsAgo1;      // keep track of direction motor turned 2 times ago
char twoTurnsAgo2;
int butCount; 			// button count timer
char onetime;           // ensure update only when max is found
int x1, y1;            	// coordinates for tv display
int oldturns1, oldturns2;                            // keep track of previous max locations
char prevTurn1, prevTurn2, turn1prev, turn2prev;    
int p, k, newturn1, newturn2, t1, t2;                // counter, increment variables
int prevx1, prevy1;                                  // previous x,y location on tv
unsigned char search1, search2;                      // search algorithm boolean variables
int searchRadius1, searchRadius2;                    // radius of search
int oldsearchRadius1, oldsearchRadius2;              // previous search radius
int j, l, m, n;
int doneLeft1, doneRight1, doneLeft2, doneRight2;     // completed search boolean variables 
int turns1, turns2;                                   // current position of motor
unsigned int time0;                                   // timer variable 
unsigned char mode;									  // mode of tv display
char cu1[]="RADAR";   
char ne[] = "NE";
char sw[] = "SW";
char nw[] = "NW";
char se[] = "SE";
char distname[] = "DIST";
char distval[10];
float alpha1, alpha2;                             // distances, angles for each motor
int d, dist, rad, ang, oldrad;
float scpt;                                       // scaling variable for tv display
unsigned char run, q, clear;
char cs[90], sn[90];							// sin cos arrays
unsigned char step, stepct1, stepct2;            // counter variables for stepper motors
int stop, found1, found2;
int maxturns1, maxturns2;


/* These reset functions are simply to scale the number of turns so that
   if the motor keeps spinning, we can still reference its current position
   relative to our coordinate system */
void resetTurns1()
{
	while (turns1 < 0)
	{
	 	turns1 = turns1 + ONE_REV;	 	
	}
	while (turns1 >= ONE_REV)
	{
		turns1 = turns1 - ONE_REV;
	}	
}   

void resetTurns2()
{ 
	while(turns2 < 0)
	{
	 	turns2 = turns2 + ONE_REV;	 	
	}
	while(turns2 >= ONE_REV)
	{
		turns2 = turns2 - ONE_REV;
	}	
} 

/* Turn Motor 1 one turn to the right */          
void motor1RS()
{
    time0=0;  
    step=0;	
   	while (step<1) 	
    {   
       	if (time0==100) // we delayed the steps to maintain smooth rotation of motor
       	{                         	
       	   if (stepct1 == 5)
       	   {
       	   		stepct1 = 1;
       	   }
       	   else
       	   {
       	   		stepct1++;
       	   }       	
       	   
     		if (stepct1 == 1)
     		{    //step 1 - + + -
      		 	 PORTB.5 = 1;
      		 	 PORTB.4 = 1;
     		}
		    else if (stepct1 == 2)
		    {	 //step 2: + - + -	 
      		 	 PORTB.5 = 0;
      		 	 PORTB.4 = 1;
		    }
   	   		else if (stepct1 == 3)
   	   		{    //step 3: + - - +
      		 	 PORTB.5 = 0;
      		 	 PORTB.4 = 0;
   	   		}
	    	else if (stepct1 == 4)
	    	{    //step 4: - + - + 
      		 	 PORTB.5 = 1;
      		 	 PORTB.4 = 0;
      		}      
      		else if (stepct1 == 5)
      		{    //step 5: - + + - 
      		 	 PORTB.5 = 1;
      		 	 PORTB.4 = 1;      		
      		}
	    	step++;
			time0=0;
		}   
	}    
	turns1--;	 
}                        

/* motor 2 turns 1 turn to the right */
void motor2RS()
{
    time0=0;  
    step=0;	
   	while (step<1) 	
    {   
       	if (time0==100) 
       	{                         	
       	   if (stepct2 == 5)
       	   {
       	   		stepct2 = 1;
       	   }
       	   else
       	   {
       	   		stepct2++;
       	   }       	
       	   
     		if (stepct2 == 1)
     		{    //step 1 - + + -
      		 	 PORTB.7 = 1;
      		 	 PORTB.6 = 1;
     		}
		    else if (stepct2 == 2)
		    {	 //step 2: + - + -	    
      		 	 PORTB.7 = 0;
      		 	 PORTB.6 = 1;
		    }
   	   		else if (stepct2 == 3)
   	   		{    //step 3: + - - +
      		 	 PORTB.7 = 0;
      		 	 PORTB.6 = 0;
   	   		}
	    	else if (stepct2 == 4)
	    	{    //step 4: - + - + 
      		 	 PORTB.7 = 1;
      		 	 PORTB.6 = 0;
      		}      
      		else if (stepct2 == 5)
      		{    //step 5: - + + - 
      		 	 PORTB.7 = 1;
      		 	 PORTB.6 = 1;      		
      		}
	    	step++;
			time0=0;
		}   
	}    
	turns2++;  	 
} 
                
/* turn motor 1 one turn to the left */
void motor1LS()
{
    time0=0;  
    step=0;	
   	while (step<1) 	
    {   
       	if (time0==100) 
       	{     
     		if (stepct1==1)
     		{
     			 stepct1=5;
     		}
     		else
     		{
     			 stepct1--;
     		}
         
	 		if (stepct1 == 1)
	 		{
	 			PORTB.5 = 1;
	 			PORTB.4 = 1;
	 		}
       		else if (stepct1 == 2)
       		{
	 			PORTB.5 = 0;
	 			PORTB.4 = 1;       		
       		} 	
  		    else if (stepct1 == 3)
  		    {  		    
	 			PORTB.5 = 0;
	 			PORTB.4 = 0;  		    
  		    } 	
  		    else if (stepct1 == 4)
  		    {
	 			PORTB.5 = 1;
	 			PORTB.4 = 0;  		    
  		    }  		                    
  		    else if (stepct1 == 5)
			{
	 			PORTB.5 = 1;
	 			PORTB.4 = 1;							
	 		}			  		      		 
  		    step++;
			time0=0;
		}   
	}    
	turns1++;   	 
}                        
        
/* turn motor 2 one turn to the left */
void motor2LS()
{
    time0=0;  
    step=0;	
   	while (step<1) 	
    {   
       	if (time0==100)
       	{     
     		if (stepct2==1)
     		{
     			 stepct2=5;
     		}
     		else
     		{
     			 stepct2--;
     		}
         
	 		if (stepct2 == 1)
	 		{
	 			PORTB.7 = 1;
	 			PORTB.6 = 1;
	 		}
       		else if (stepct2 == 2)
       		{
	 			PORTB.7 = 0;
	 			PORTB.6 = 1;       		
       		} 	
  		    else if (stepct2 == 3)
  		    {  		    
	 			PORTB.7 = 0;
	 			PORTB.6 = 0;  		    
  		    } 	
  		    else if (stepct2 == 4)
  		    {
	 			PORTB.7 = 1;
	 			PORTB.6 = 0;  		    
  		    }  		                    
  		    else if (stepct2 == 5)
			{
	 			PORTB.7 = 1;
	 			PORTB.6 = 1;							
	 		}			  		      		 
  		    step++;
			time0=0;
		}   
	}    
	turns2--;  	 
}    

//This is the sync generator and raster generator. It MUST be entered from 
//sleep mode to get accurate timing of the sync pulses
#pragma warn-
interrupt [TIM1_COMPA] void t1_cmpA(void)  
begin 
  //start the Horizontal sync pulse    
  time0++;
  butCount++; 
  
/* REMOVED LAB 4  TV CODE HERE */
	
 	// determine which button was pressed   
    if (butCount >= 2210)
    {         	    
	      if (PINA.2 == 0)
	      {	// Button A.2 is run
	  		run = run ^ 0x01;	    		
	  		if (run == 0) stop = 0;          			
 		    PORTA.6 = PORTA.6 ^ 0x01;
	      }
	      if (PINA.3 == 0) 
	      {  
	  		// switch modes
	  		mode = mode ^ 0x01; 
	  		//PORTA.7 = PORTA.7 ^ 0x01;   
	      }
	      butCount = 0;         	      
	  } 	  
end  
#pragma warn+ 

// set up the ports and timers
void main(void)
begin 
  //init timer 1 to generate sync
  OCR1A = lineTime; 	//One NTSC line
  TCCR1B = 9; 		//full speed; clear-on-match
  TCCR1A = 0x00;	//turn off pwm and oc lines
  TIMSK = 0x10;		//enable interrupt T1 cmp 
   
    //init ports
  DDRD = 0xff;		//video out and switches           
  DDRB = 0xff; 
  //DDRC = 0xff;
  DDRA = 0b11000000;
  PORTA = 0b11001111;           
  PORTA.4 = 0;
  PORTA.5 = 0;
  //D.5 is sync:1000 ohm + diode to 75 ohm resistor
  //D.6 is video:330 ohm + diode to 75 ohm resistor
  
  //initialize synch constants 
  LineCount = 1;
  syncON = 0b00000000;
  syncOFF = 0b10000000;    
  
  lcd_init(LCDwidth);      	//initialize the display
  lcd_clear();       		//clear the display
   
  //side lines 
  //#define width 126
 // video_line(0,0,0,99,1);
  //video_line(width,0,width,99,1);
  
  //top line & bottom lines
 // video_line(0,0,width,0,1);
 // video_line(0,99,width,99,1);
  //video_line(0,11,width,11,1);
  //video_line(0,89,width,89,1);
    
  //init software timer and other variables
  t=0;  
  time0 = 0;
  butCount = 0;
  alpha1 = 0.0;
  alpha2 = 0.0;
  x1 = 0;
  y1 = 0;   
  prevx1 = 0;
  prevy1 = 0;    
  scpt = 1.0;
  d = 65;                  
  rad = 30; 
  oldrad = 0;
  clear = 0; 
  mode = 0;     
  stop = 0;    
  onetime = 1;
  turns1 = 0;
  turns2 = 0;
  maxturns1 = 0;
  maxturns2 = 0;  
  //CurSeekMode = INIT;
  run = 0;     
  stepct1 = 1;
  stepct2 = 1;  
  prevTurn1 = LEFT;
  prevTurn2 = RIGHT;
  turn1prev = NOPREV;
  turn2prev = NOPREV;
  oldturns1 = -5;
  oldturns2 = -5;    
  found1=0;
  found2=0;              
  oldsearchRadius1 = 10;
  oldsearchRadius2 = 10;    
  osc1 = 0;
  osc2 = 0;                
  twoTurnsAgo1 = NOPREV;
  twoTurnsAgo2 = NOPREV;
  /*for (ang = 0; ang < 90; ang++)
  {
    	cs[ang]=100*cos(ang*3.14/180);
    	sn[ang]=100*sin(ang*3.14/180);	    
  }                                 
  
  for (ang = 0; ang < 90; ang=ang+10)
  {    
   		video_pt(CENTERX+rad*cs[ang]/100, CENTERY+rad*sn[ang]/100,2);	    
  		video_pt(CENTERX-rad*cs[ang]/100, CENTERY-rad*sn[ang]/100,2);	    
  		video_pt(CENTERX-rad*cs[ang]/100, CENTERY+rad*sn[ang]/100,2);	    
  		video_pt(CENTERX+rad*cs[ang]/100, CENTERY-rad*sn[ang]/100,2);	    
  }  */
  
  //enable sleep mode
  MCUCR = 0b10000000;
  #asm ("sei");
  
  //The following loop executes once/video line during lines
  //1-230, then does all of the frame-end processing
  while(1)
  begin
    //stall here until next line starts
    //sleep enable; mode=idle  
    //use sleep to make entry into sync ISR uniform time  
    #asm ("sleep");                                       
    if (LineCount == 231)
    begin	    	
    	if (run == 0)
    	{    	
    		resetTurns1();
	  		resetTurns2();
	  	   	while (turns1 > 0 || turns2 > 0)
	  		{
	  			if (turns1 > 0)	motor1RS();
	  			if (turns2 > 0) motor2LS();
	  		}  
	  		stop = 0;	  
	  	}                  	
    	else if (run == 1)
    	{   	   	    
			while ((stop < ONE_REV) && (run == 1))
			{	
		   		// turn motor  1 left
		        if (found1==0)
		        {
		        	motor1LS();
    	        }
			    // is max found on motor 1?
		  		if (PINA.0 == 0)
		  		{
					//maxturns1 = turns1;
					found1=1;   
					resetTurns1();
  				}
		        if (found2==0)
		        {
					motor2RS(); 
				}                                                     
		  		if (PINA.1 == 0) // is max found on motor 2?
		  		{
					//maxturns2 = turns2; 
					found2=1;
					resetTurns2();
  				}                    			   
		  		stop++;    				
	  		}    
           
		    //scan if lost signal    	    	    	   
    		if (stop == ONE_REV)
    		{   
    			// flash center point and init variables
	    		video_pt(CENTERX, CENTERY, 2);  
				search1 = 0;	 				               
	  	    	search2 = 0; 
	 		    searchRadius1 = 2;   
			    searchRadius2 = 2; 	
			    oldsearchRadius1 = 1;
			    oldsearchRadius1 = 1;	
			    osc1 = 0;
			    osc2 = 0;		
			    j = 0;  
			    k = 0;
			    m = 0;  
		 	    n = 0;
			    doneLeft1 = 0;
		    	doneRight1 = 1;	 			
			    doneLeft2 = 1;
			    doneRight2 = 0;	 	    		
	 			//lost object, search to find it 							                	 				
		 		while ((search1==0 || search2==0) && (run == 1))
		 		{   	 		                            	 		   									
			 		// read IR reciever value on motor 1	 				
					if (PINA.0 == 0)
					{
						 search1 = 1;    
						 j = 0;
						 m = 0;     
						 oldsearchRadius1 = searchRadius1;
						 searchRadius1 = 2;    
						 twoTurnsAgo1 = prevTurn1;
						 prevTurn1 = turn1prev;
					} 
					else
					{
						search1 = 0;
					}
					// check motor 2 signal value
					if (PINA.1 == 0)
					{
						 search2 = 1;    
						 n = 0;
						 k = 0;               
						 oldsearchRadius2 = searchRadius2;
						 searchRadius2 = 2;  					
						 twoTurnsAgo2 = prevTurn2;
						 prevTurn2 = turn2prev;
					}    
					else
					{
						search2 = 0;
					}                                                    
					// motor oscillation damping code
				   /*	if (twoTurnsAgo1 == LEFT && prevTurn1 == RIGHT && (searchRadius1 == oldsearchRadius1))
					{
						osc1 = 1;
					}    
					if (twoTurnsAgo1 == RIGHT && prevTurn1 == LEFT && (searchRadius1 == oldsearchRadius1))
					{
						osc1 = 1;
					}               
					if (twoTurnsAgo2 == LEFT && prevTurn2 == RIGHT && (searchRadius2 == oldsearchRadius2))
					{
						osc2 = 1;
					}                      
					if (twoTurnsAgo2 == RIGHT && prevTurn2 == LEFT && (searchRadius2 == oldsearchRadius2))
					{
						osc2 = 1;
					} */          
					// set previous turn variables            					
					if (prevTurn1 == LEFT)
					{
						doneRight1 = 1;
						doneLeft1 = 0; 
						twoTurnsAgo1 = prevTurn1;
						prevTurn1 = NOPREV;
					}   	
					else if (prevTurn1 == RIGHT)
					{
						doneRight1 = 0;
						doneLeft1 = 1;       
						twoTurnsAgo1 = prevTurn1;
						prevTurn1 = NOPREV;
					}
					// motor 2 heuristic tracking
					if (prevTurn2 == LEFT)
					{
						doneRight2 = 0;
						doneLeft2 = 1;     
						twoTurnsAgo2 = prevTurn2;
						prevTurn2 = NOPREV;
					}
					else if (prevTurn2 == RIGHT)
					{
					 	doneRight2 = 1;
					 	doneLeft2 = 0;      
					 	twoTurnsAgo2 = prevTurn2;
					 	prevTurn2 = NOPREV;
					}
				    // determine if max is in left plane of motor 1
			 		if (j < searchRadius1 && search1==0 && doneRight1==1)
			 		{
					 	// turn motor 1 to the left       
					 	//if (osc1 == 0)
					 	//{
		 				 	motor1LS();	     
		 				//}
		 			 	turn1prev = LEFT;			 	
						j++;              
					}   
					// max not found going left, go right
		 			if (j >= searchRadius1 && doneRight1==1) 
		 			{
			 			doneLeft1 = 1;
						doneRight1 = 0; 
						j = 0;
 				    	searchRadius1 = searchRadius1 + 2;
	                } 
	                // determine if max is in right plane of motor 2
			 		if (k < searchRadius2 && search2==0 && doneRight2==1)
					{
		 		 		// turn motor 2 to the left
		 		 		//if (osc2 == 0)
		 		 		//{
			 			 	motor2RS();   
			 			 //}
	 				 	turn2prev = RIGHT;
						k++;
			 		}            
			 		// max not found, go left
			 		if (k >= searchRadius2  && doneRight2==1)
			 		{	
						doneLeft2 = 1;
						doneRight2 = 0; 
						k = 0;
 					    searchRadius2 = searchRadius2 + 2;
			 		}
					
					// read signal value on motor 1	 				
					if (PINA.0 == 0)
					{
						 search1 = 1;    
						 j = 0;
						 m = 0;  
						 searchRadius1 = 2;
						 prevTurn1 = turn1prev;
					}    
					else
					{
						search1 = 0;
					}
    				// now turn motor 1 to the right and seek signal
					if (m < searchRadius1 && search1==0 && doneLeft1==1)
					{
					 	// turn motor 1 to the right   
					   //	if (osc1 == 0)
					 	//{
		 				 	motor1RS();   
		 				//}
	 				 	turn1prev = RIGHT;
					 	m++;	 			 	
			 		}       
			 		// turn motor 1 to the left now
					if (m >= searchRadius1 && doneLeft1==1)
					{
			 			doneLeft1 = 0;
		 				doneRight1 = 1;    
			 			m = 0;
 				    	searchRadius1 = searchRadius1 + 2;
					}
		 			// turn motor 2 to the left, seek signal
					if (n < searchRadius2 && search2==0 && doneLeft2==1)
					{
	 			 		// turn motor 2 to the right       
	 			 		//if (osc2 == 0)
	 			 		//{
			 			 	motor2LS();          
			 			//}
		 			 	turn2prev = LEFT;
	 				 	n++;
		 			}  
		 			// turn motor 2 to the right, seek signal
			 		if (n >= searchRadius2 && doneLeft2==1)
			 		{	
						doneLeft2 = 0;
						doneRight2 = 1;    
						n = 0;				
	 			    	searchRadius2 = searchRadius2 + 2;
			 		}
			 		// check motor 2
					if (PINA.1 == 0)
					{
						 search2 = 1;    
						 n = 0;
						 k = 0;  
						 searchRadius2 = 2; 
						 prevTurn2 = turn2prev;
					}   
					else
					{
						search2 = 0;
					}            						 	 				
					 // default case below, object has gone beyond our tracking ability
					 if (searchRadius1 > ONE_REV)
					 {
					 	 searchRadius1 = 2;
					 }                
					 if (searchRadius2 > ONE_REV)
					 {
					     searchRadius2 = 2;
					 }    					 
				}
				// new max has been found, set flag
				if (oldturns1 != turns1 || oldturns2 != turns2)
				{
				 	onetime = 1;  				 	
				}			
				//delay_ms(50);
 			    if (onetime ==  1)
 			    {    
 			    	// reset turns to coordinate system and display output to TV			    
					resetTurns1();
					resetTurns2();	 			               
	    			alpha1 = (turns1*6.28)/ONE_REV;
    			    alpha2 = (turns2*6.28)/ONE_REV;  
	    		   	//alpha1 = alpha2 = 45 *PI/180;
	    		   	//alpha2 = alpha2+0.628/180; 				
				   	if ( turns1 == 0 && turns2 == 0 || (turns1 == ONE_REV/2 && turns2 == 0) || (turns1 == 0 && turns2 == ONE_REV/2))
					{
					   	x1 = 0;
					   	y1 =  0;
					}  
					else
					{   // use triangulation to determine x, y location 
		   			    x1 = (int)floor(0.5+d*((sin(alpha2)*cos(alpha1))/(sin(alpha1+alpha2)) - 0.5));
	   				    y1 = (int)floor(0.5+(d*sin(alpha2)*sin(alpha1))/(sin(alpha1+alpha2)));	   
			   		}  	  	   			
			   		dist = (int) floor(0.5+sqrt(pow(x1,2.0)+pow(y1,2.0))); 
				    if (mode == 1)
	    		    {    // display trajectory 	    		    	
						video_pt( (int)(x1/2+CENTERX), (int)(CENTERY-y1/2), 1);	    	     	   	                                        
					}
					else  				
					{    // show only current location                                     		
					  //	video_pt( (int)(prevx1/2+CENTERX), (int)(CENTERY-prevy1/2), 0);
						video_pt( (int)(x1/2+CENTERX), (int)(CENTERY-y1/2), 1);              		
						prevx1 = x1; 
						prevy1 = y1;       
					   
					  /* if ((int)(scpt*x1) > CENTERX || (int)(scpt*y1) > CENTERY) 
						{
							scpt = scpt/10;
						}*/                 
					}     
					/* DISPLAY DISTANCE ON TV */    
					//sprintf(distval,"%d",dist);
					//video_putsmalls(60,93, distval);
					/* DISPLAY DIRECTION ON TV
					if (x1 >= 0 && y1 >= 0)	{
					   video_putsmalls(12,12,ne);
					} else if (x1 >= 0 && y1 < 0) {
						video_putsmalls(12,12,se);
					} else if (x1 < 0 && y1 <= 0) {
						video_putsmalls(12,12,sw);
					} else {
						video_putsmalls(12,12,nw);
					} */      			         
					      
					/* DISPLAY DEBUG INFO ON LCD */
					//lcd_clear();				
				   	lcd_clear();				
					lcd_gotoxy(0,0); 
					sprintf(lcd_buffer,"%d",dist);
					lcd_puts(lcd_buffer); 
				    lcd_gotoxy(0,5);
					lcd_putsf("**"); 
				    lcd_gotoxy(0,8);
					sprintf(lcd_buffer,"%d",(int)alpha1);
					lcd_puts(lcd_buffer);    */ 

   					//DISPLAY FINAL OUTPUT TO LCD 	 
   					lcd_clear();
				   	lcd_gotoxy(0,0);
					lcd_putsf("dist=");                                         
					lcd_gotoxy(0,5);     
					sprintf(lcd_buffer,"%d",dist);
					lcd_puts(lcd_buffer);
				   //	delay_ms(10);
					lcd_gotoxy(0,11); 					   					
					if (x1 >= 0 && y1 >= 0)	{
						lcd_putsf("NE"); 
					} else if (x1 >= 0 && y1 < 0) {
						lcd_putsf("SE");
					} else if (x1 < 0 && y1 <= 0) {
						lcd_putsf("SW");
					} else {
						lcd_putsf("NW");	
					}             
					if (dist > 43)
					{
						PORTA.7 = 0;
					}               
					else
					{
						PORTA.7 = 1;
					}
				   	onetime = 0; 						
			  	   	oldturns1 = turns1;
			 		oldturns2 = turns2;	 			 		 
				}
			}		
		}
	end  	       
  end  //while
end  //main