// Functions used for Dinsmore Compass Calculations.
// Operational on bot but not tested here.  Separate test program
// not developed.

// To use, read_analog_points then calc_compass, then printout compass_head and dCompass.

#define PI_OVER_2 1.571 
#define PI 3.1416
#define TWO_PI 6.2832
#define  X180OVER_PI  57.294

#define COMPASS_UPPER 530			// Upper calc limit - at upper point where two curves cross
#define COMPASS_LOWER 339			// Upper calc limit - at lower point where two curves cross#define COMPASS_ADJUST  0			// Offset from 0 adjust

// Dinsmore Compass defines used for sin/cos method
#define COMPASS_MID 439             // Midpoint average of both curves
#define COMPASS_RANGE 287			// Range from high point to low point
#define COMPASS_ADJUST  0			// Offset from 0 adjust

#define DIN_TABLE_SIZE  7
INT16S lin_din_rising(INT16S xin, INT16S table[2][DIN_TABLE_SIZE]);
INT16S lin_din_falling(INT16S xin, INT16S table[2][DIN_TABLE_SIZE]);

INT16S din_table1 [2][DIN_TABLE_SIZE] = { 
	{574,541,499,430,340,321,305}, 
	{300,315,330,360,405,420,434}};
					
INT16S din_table2 [2][DIN_TABLE_SIZE] = { 
	{537,522,498,460,383,341,302}, 
	{ 60, 74, 90,106,120,127,123}};
	
INT16S din_table3 [2][DIN_TABLE_SIZE] = { 
	{339,374,431,495,531,545,564}, 
	{127,135,150,180,210,225,240}};
					
INT16S din_table4 [2][DIN_TABLE_SIZE] = { 
	{313,339,356,381,436,517,553}, 
	{180,210,225,240,270,300,315}};

int compass1, compass2;
double compass_heading, compass_head;
INT16S dCompass;

// Reads a single analog pin, mux = 0 thru 7 same as pin
INT16U read_analog(CHARU mux)
   {
   ADMUX = mux;
   ADCSR = 0xD7;     // Enables, starts conversion, resets done flag
   while(bit_is_clear(ADCSR, 4));     // Wait until conversion is complete
	return (INT16U)(ADCL | ((int)ADCH<<8));
   }

void read_analog_points(void)
	{
    compass1 = read_analog(3);
    compass2 = read_analog(4);
	}
	
/*  Dinsmore Compass Calculations - sin/cos method
     takes two analog inputs - one SIN, one COS of theta */    
void calc_compass(void)
   {
	int sector;   
	if (compass1 > COMPASS_MID && compass2 > COMPASS_MID)  /* Determine Sector */
		sector = 1;
	else if (compass1 > COMPASS_MID && compass2 < COMPASS_MID)
			sector = 2;
			else if (compass1 < COMPASS_MID && compass2 < COMPASS_MID)
				sector = 3;
				else 
					sector = 4;
					
	// if using SIN curve 	
	if ((compass1 <= COMPASS_UPPER) && (compass1 >= COMPASS_UPPER)) {
		compass_heading = asin(((double)(compass1 - COMPASS_MID)) / COMPASS_RANGE);
		switch (sector) {
			case 2:
			case 3:
				compass_heading = PI - compass_heading;
				break;
			case 4:
				compass_heading = TWO_PI + compass_heading;
				break;
			}
		}
	else  {												/* if using COS curve */
		compass_heading = acos(((double)(compass2 - COMPASS_MID)) / COMPASS_RANGE);
		switch (sector) {
			case 3:
			case 4:
				compass_heading = TWO_PI - compass_heading;
				break;
			}
		}
		
	compass_head =  COMPASS_ADJUST + (int)(compass_heading * 57.29);  // * X180OVER_PI);
	if (compass_head > 360) compass_head = compass_head - 360;
	dCompass = compass_head;

   }
   
//  Dinsmore Compass Calculation - Lookup table method
//  Takes two analog inputs - compass 1, compass2
//  Based on the two values determine which sector to use.
//  This defines which lookup table is to be used to determine the actual heading.
void calc_compass(void)
   {
	CHARU sector;

	// Determine Sector
    if(compass1 > COMPASS_UPPER)
        sector = 1;
    else if (compass2 > COMPASS_UPPER)
        sector = 4;
    else if (compass1 < COMPASS_LOWER)
        sector = 3;
    else
        sector = 2;

	switch (sector)
		{
		case 1:
			dCompass = lin_din_falling(compass2, din_table1);
			break;
		
		case 2:
			dCompass = lin_din_falling(compass1, din_table2);
			break;
			
		case 3:
			dCompass = lin_din_rising(compass2, din_table3);
			break;
		
		case 4:
			dCompass = lin_din_rising(compass1, din_table4);
			break;
		}
    if(dCompass >= 360)
        dCompass -= 360;
        
    heading = dCompass;     // This is the variable used in the calculations.
   }           	
   
// Table look-up linearize routine for 0 to 360 degree compass values
// for Dinsmore Compass
// Based on a fixed table size of DIN_TABLE_SIZE.
// Input table values must continuously fall from [0] to [DIN_TABLE_SIZE]
INT16S lin_din_falling(INT16S xin, INT16S table[2][DIN_TABLE_SIZE])
	{
	CHARU j;
	INT16S xout;
	for (j = 1; j < DIN_TABLE_SIZE; j++)
		{
		if (xin > table[0][j])
			{
			xout =table[1][j-1] - (((((table[0][j-1] - xin)*256) /(table[0][j-1] - 
					table[0][j])) * (table[1][j-1] - table[1][j])) / 256);
			break;
			}
		else
			xout = 999;
        }
	return xout;
	}
	
// Table look-up linearize routine for 0 to 360 degree compass values
// for Dinsmore Compass
// Based on a fixed table size of DIN_TABLE_SIZE.
// Input table values must continuously rise from [0] to [DIN_TABLE_SIZE]
INT16S lin_din_rising(INT16S xin, INT16S table[2][DIN_TABLE_SIZE])
	{
	CHARU j;
	INT16S xout;
	for (j = 1; j < DIN_TABLE_SIZE; j++)
		{
		if (xin < table[0][j])
			{
			xout =(((((xin - table[0][j-1])*256) /(table[0][j] - table[0][j-1])) *
			      (table[1][j] - table[1][j-1])) / 256) + table[1][j-1];
			break;
			}
		else
			xout = 888;
        }
	return xout;
	}