// ******************************************************************
//
//   Robot_main.c 
//   top-level controller code for robot
//
// ******************************************************************

//libraries
#include 
#include "ad_lib.h"
#include "kbhit.h"
#include "shiftreg.h"

#include "IR_read.h"
#include "RotateTurret.h"
#include "Ultrasound_Sample.h"
#include "Catapult.h"
#include "CalcShootDistance.h"

//Defines
#define DEBUG 1 //to get debugging output, set to 1 and uncomment appropriate lines
#define ULTRASONIC_SWEEP_HALF_ANGLE 30 //angle for half the US sweep, in degrees
#define ULTRASONIC_SAMPLE_ANGLE 1 //desired degrees between US samples
#define STEPPER_STEPS_PER_TURRET_SWEEP_DEGREE 300 //total guess, depends on gearing
#define STEPPER_STEPS_TO_SLOWDOWN 100 //total guess, number of steps to go slow to hedge against overshooting

//Prototypes
signed int IR_read(void);
signed int RotateTurret(signed int, unsigned int);
signed int ultrasonicSweep(void);
unsigned int Ultrasound_Sample(void);
unsigned int getUltrasoundSample(unsigned int samplesToAverage);
//void Catapult(unsigned char);
double CalcShootDistance(signed int, unsigned int, unsigned int);

//global variables
static signed int AngularPosition;
static signed int minimumDistanceAngularPosition;
static unsigned int minimumDistanceSampled;
static unsigned int samplesToAverage = 1;




//the modules
void main(void)
{

unsigned char theKey;
signed int rotation;
signed int lastRotation;
unsigned char direction;
double distance;
unsigned char targeted;
signed int TargetedAngularPosition;
unsigned char IRcounter;
unsigned char NewBasket;
NewBasket = 1;
IRcounter = 0;
AngularPosition = 0;
TargetedAngularPosition = 0;
distance = 77;
rotation = 0;
lastRotation = 0;
targeted = 0;

AD_Init();



//wait for Go button to be pressed:
while ((ReadPORTF() & 0x08) == 0) { printf("poised for action...\n"); }
//the above checks port F, pin 3.
printf("time to kick ass.\n");
// 
 


//iteratively look for IR and home in on it:
while(targeted == 0){  
	//FIND IR

	rotation = IR_read();
    if (DEBUG) printf ("rotation= %d\n", rotation); 

    /*  // the following bit is to operate the stepper with the keyboard...
        if (kbhit() ) { theKey = getchar(); 
        if (theKey == '1') { rotation = 1;}
        else if (theKey == '2') { rotation = 2;}
        else if (theKey == '3') { rotation = -2;}
        else if (theKey == '4') { rotation = -1;}
        else if (theKey == '0') { rotation = 0;}   		   		   		
        }	 
    //    */

    AngularPosition = RotateTurret(rotation, 100);
    if (DEBUG) printf("AngularPosition= %d", AngularPosition);
	if (rotation == 0) { 
       	targeted = 1; 
    	TargetedAngularPosition = AngularPosition;}
}	



//ULTRASONIC sweep.  the angular position is a global variable
if (DEBUG) printf("running ultrasonic sweep...");

ultrasonicSweep();

if (DEBUG) printf("\n\n *** ultrasonicSweep finished. STATS:");
if (DEBUG) printf("\n min dist ang position: %d, dmin dist: %u", 
minimumDistanceAngularPosition, minimumDistanceSampled);
if (DEBUG) printf("\n current ang position: %u", AngularPosition);
	
		
		
//move robot back to where IR was:  
while(TargetedAngularPosition > AngularPosition + 50) {  // the 50 is because we're moving 100 steps at a time; AngularPosition may never be exactly equal to TargetedAngularPosition.  Avoids looping forever.
    AngularPosition = RotateTurret(-2, 100);
}
while(TargetedAngularPosition < AngularPosition - 50) { // the 50... ditto above.
    AngularPosition = RotateTurret(2, 100);
}
	targeted = 0;  // tells robot IR is not found yet

	
	
//re-find IR and aim at basket:
NewBasket = 1;
while(1)
{
    while(targeted == 0){
    	lastRotation = rotation;
    	rotation = IR_read();
        AngularPosition = RotateTurret(rotation, 100);
    	if (rotation == 0) 
		{
    		if (NewBasket == 1)
			{
    			AngularPosition = RotateTurret(lastRotation, 100); 
        		if (DEBUG) printf("***last rotation before shooting %d, ang pos= %d", lastRotation, AngularPosition);//keep going a little bit more  	
        	}
        	NewBasket = 0;
			targeted = 1; 
		}
    	else if (rotation != 0)
		{
		 	NewBasket = 1;
		}
    }	

	//shoot:
	distance = CalcShootDistance(AngularPosition, minimumDistanceAngularPosition, minimumDistanceSampled);
	Catapult(distance);
	targeted = 0;
	if (DEBUG) printf("ball shot.");
    	    
    

}  //end of shoot-reacquire while loop

printf("\n goin' to sleep.");
	
	

 //end of main
}




/***  After main subroutines ***/
 
 
signed int ultrasonicSweep(void) 
{

unsigned int wallDistance[(ULTRASONIC_SWEEP_HALF_ANGLE*2)/ULTRASONIC_SAMPLE_ANGLE]; //size the array to contain all the needed elements
unsigned int ultrasoundSampleAngularPosition[(ULTRASONIC_SWEEP_HALF_ANGLE*2)/ULTRASONIC_SAMPLE_ANGLE]; //same size -- faster than 2D array
unsigned int ultrasonicSweptAngle;
unsigned int ultrasoundSampleNumber;
//unsigned int minimumDistanceAngularPosition;
//unsigned int minimumDistanceSampled;
minimumDistanceSampled = 27777;

//move turret to extreme of sweep, fast then slow, RotateTurret keeps track of position for us
AngularPosition = RotateTurret (2, (ULTRASONIC_SWEEP_HALF_ANGLE*STEPPER_STEPS_PER_TURRET_SWEEP_DEGREE - STEPPER_STEPS_TO_SLOWDOWN)) ;
	if (DEBUG) printf("  rotateturret 2, %d", (ULTRASONIC_SWEEP_HALF_ANGLE*STEPPER_STEPS_PER_TURRET_SWEEP_DEGREE - STEPPER_STEPS_TO_SLOWDOWN));
	
AngularPosition = RotateTurret (1, STEPPER_STEPS_TO_SLOWDOWN);
if (DEBUG) printf("  rotateturret 1, %d", STEPPER_STEPS_TO_SLOWDOWN);
ultrasonicSweptAngle = 0;
ultrasoundSampleNumber = 0; //counter for US samples

//while (ultrasonicSweptAngle <= (2*ULTRASONIC_SWEEP_HALF_ANGLE)) {
while (ultrasonicSweptAngle <= 42) {
    //sample
	
	//if (DEBUG) printf("US Sample angle: %u", ultrasonicSweptAngle);
	wallDistance[ultrasoundSampleNumber] = getUltrasoundSample(samplesToAverage);
	if (DEBUG) printf("\n AngularPosition= %d", AngularPosition);
	
	//if (DEBUG) printf("Wall dist: %u", wallDistance[ultrasoundSampleNumber]);
	if (wallDistance[ultrasoundSampleNumber] < minimumDistanceSampled) {
	    minimumDistanceAngularPosition = AngularPosition;
		minimumDistanceSampled = wallDistance[ultrasoundSampleNumber];
		if (DEBUG) printf("This is the Minimum samp#: %u,  ang: %d, dist: %u", ultrasoundSampleNumber, minimumDistanceAngularPosition, minimumDistanceSampled);
    	}
    //step and record AngularPosition
	//if (DEBUG) printf("calling rotate turret 1 %u", ULTRASONIC_SAMPLE_ANGLE*STEPPER_STEPS_PER_TURRET_SWEEP_DEGREE);
	ultrasoundSampleAngularPosition[ultrasoundSampleNumber] = AngularPosition;
    ultrasonicSweptAngle = ultrasonicSweptAngle + ULTRASONIC_SAMPLE_ANGLE;
	if (DEBUG) printf("\n the Minimum distance ang: %d, dist: %u", minimumDistanceAngularPosition, minimumDistanceSampled);
	//Rotate
	AngularPosition = RotateTurret (1, ULTRASONIC_SAMPLE_ANGLE*STEPPER_STEPS_PER_TURRET_SWEEP_DEGREE);
	//if (DEBUG) printf("\nswept angle: %u", ultrasonicSweptAngle);
	ultrasoundSampleNumber = ultrasoundSampleNumber +1;

    }

return 1;
}