//**********************************************
//         Distance Calculation Routine
// version 1.0
// takes input of signed int AngularPosition, unsigned int 
// minimumDistanceAngularPosition, unsigned int minimumDistanceSampled,
// and gives output double ShootDistance by calculating it. 
//
//**********************************************

//libraries
#include 

//defines
#define DEBUG 1 //to get debugging output, set to 1 and uncomment appropriate lines

//global variables -- comment out for module
//signed int AngularPosition;
//unsigned int MinimumDistanceAngularPosition;
//unsigned int minimumDistanceSampled;


//void main(void)  
double CalcShootDistance(signed int AngularPosition, unsigned int minimumDistanceAngularPosition, unsigned int minimumDistanceSampled)
{

unsigned int CourtWidth = 244;  // the width of the court (8ft.) in cm
double ShootDistance;
double ShootAngle;  
unsigned int StepsPerRevolution = 5100;  //conversion from motor steps to revolutions for angle, measured by Jer Saturday

//dummy values for variables:
    //AngularPosition = StepsPerRadian*30;  //testing a 30 degree angle
    //MinimumDistanceAngularPosition = 0;
    //minimumDistanceSampled = 777;

	   ShootAngle = abs( ((AngularPosition - minimumDistanceAngularPosition)*2*3.14159/StepsPerRevolution) );  //this is absolute value of angle in radians
	   ShootDistance = ( (CourtWidth - minimumDistanceSampled/100)/cos(ShootAngle) );  //the /100 is to convert from .1mm scale to 1cm scale.


        //print all values for debugging:		   
        if (DEBUG) 
    		{
    		printf ("\n\n AngPos= %d", (int)AngularPosition); 
            printf ("   MinAngPos= %d", (int)minimumDistanceAngularPosition);
            printf ("\n dist from bot to far wall= %d", (int)(CourtWidth - minimumDistanceSampled/100) );
            printf ("   ShootAngle= %d \n", (int)ShootAngle*180/3.141);  //printed in degrees, not radians
			printf ("   ShootDistance= %d \n", (int)ShootDistance);
    		}

return ShootDistance; 
		

 
 
//end
}