#include "C:\Program Files\PICC\ASprojects\Radio_Orienteering\Tracker.h"
#include "C:\Program Files\PICC\ASprojects\Radio_Orienteering\LCD_SPLC780D.c"



#int_RDA    // RS232 received data
RDA_isr()
{

   gen1=0;

}

void getreading()
{  delay_ms(2);
   advalue=0;
   for(gen=1;gen<11;gen++)
      {  delay_us(20);
         advalue = advalue + read_adc();
      }
   advalue=advalue/10.0;

}

void setup()
{  port_b_pullups(TRUE);
   setup_adc_ports(AN0|VSS_VDD);
   setup_adc(ADC_CLOCK_INTERNAL);
   setup_spi(FALSE);
   setup_wdt(WDT_OFF);
   setup_timer_0 (RTCC_DIV_256|RTCC_INTERNAL);  // Increment every 51.2uS. 0.1sec=1953
//   setup_timer_1(T1_INTERNAL | T1_DIV_BY_1);
   setup_timer_1(T1_DISABLED);
   setup_timer_2(T2_DISABLED,195,16);
 //  setup_timer_2(T2_DIV_BY_16,195,16);
   setup_timer_3(T3_DISABLED|T3_DIV_BY_8);
   setup_comparator(NC_NC_NC_NC);
   setup_vref(FALSE);
   setup_low_volt_detect(FALSE);
   setup_oscillator(OSC_4MHZ | OSC_PLL_ON);


   set_tris_a(0b00000001);
   set_tris_b(0b11111111);
   set_tris_c(0b10010000);      // need to change this when connecting compass
   delay_ms(500);
   LCD_init();


   delay_ms(200);
   GOTREFXY=0;
}

void RS232_GPS() //  Constant poll for trigger swpress
{  unsigned int16 tim;
   int1 z,allowinc=0;
   int16 index,quiet;
//   timeout_error=FALSE;
   tim=0;
   index=0;
   quiet=0;
   numchrs=0;

gotchar:
   buffer[index]=fgetc(GPS);
   numchrs++;
   if (buffer[index]==82) allowinc=1;  // look for an R
   if (allowinc && index<MAXBUFFER) index++;
awaitchar:
   if ((!input(TRIGGER)) && SECTIMER>1) SWFLAG=1;  // If trigger switch pressed activate flag
   if (!input(SW2)) SW2FLAG=1;  // If SW2 activate flag
   delay_us(100);
   quiet++;
   if (quiet>200)  // if receive nothing for 20mS then exit
   {  nochrs=index;
      return;
   }
   z=kbhit(GPS); // now check for second and subseqent characters
   if (z==1)
   {  quiet=0;
      goto gotchar;
   }
   goto awaitchar;

}


signed int16 NORMBEARING(signed int16 input)
{  if (input>359) return(input-360);
   if (input<0) return(input+360);
   return input;
}


void  get_bearing_distance(signed int16 CURRENTX, signed int16 CURRENTY,signed int16 X,signed int16 Y) // get the Magnetic bearing from current point to best guess point
{  float XF,YF;
   float DIST;
   XF=X-CURRENTX;
   YF=Y-CURRENTY;
   genf=ATAN2(YF,XF);
   genf=genf/degtorad;
   MAGBEARING=genf;
   MAGBEARING=(90-MAGBEARING)-MAG_VARIATION; // MB=TB-MV
   MAGBEARING=NORMBEARING(MAGBEARING); // put to 0 to 359
   DIST = sqrt((XF*XF) + YF*YF);
   DISTANCE=DIST;
}

void get_bestxy()
{  float xi,yi,mi,xj,yj,mj;
   int8 p;
   signed int8 kk;
   // At this stage NUMB[TXNUM]=number of bearing lines pointing to a certain TX
   // and BEARINDEX[TXNUM]=index of the most recent line. i.e if 4 then 3,2,1,0 are older or if NUMB[TXNUM]>4 then 3,2,1,0,6.. are older
   BESTXYVALID[TXNUM]=1; // assume its OK
   for(i=0;i<(NUMB[TXNUM]);i++)  // loop number of bearings times
   {  if (IGNORE[TXNUM][i]==0)  // only do this if the point isn't to be ignored
      {  TB[TXNUM][i]=MB[TXNUM][i]+MAG_VARIATION;   // Note that TB could be a bit negative or be a bit bigger than 360 degrees
         TB[TXNUM][i]=NORMBEARING(TB[TXNUM][i]);  // Convert from 0 to 359
         M[TXNUM][i]=tan(degtorad*(270-TB[TXNUM][i]));  // find the gradient for each bearing
      }
   }
   // Now find the cross points between valid bearings.

   if (NUMB[TXNUM]<2)  // no crosses, cant create a best guess
   {  BESTX[TXNUM]=32000;
      BESTY[TXNUM]=32000; // doesn't exist
      goto skipbest;
   }


   for(i=0;i<(NUMB[TXNUM]-1);i++)  // loop (bearings-1) times // this is used to find all cross points
   {  for(j=(i+1);j<(NUMB[TXNUM]);j++) // this is used to find all cross points
      {  VALID=0;
         if (IGNORE[TXNUM][i]==1) goto invalid; // bearing i
         if (IGNORE[TXNUM][j]==1) goto invalid; // bearing j
         VALID=1; // Assume the X and Y are going to be valid
         xi=BSX[TXNUM][i];
         yi=BSY[TXNUM][i];
         mi=M[TXNUM][i];
         xj=BSX[TXNUM][j];
         yj=BSY[TXNUM][j];
         mj=M[TXNUM][j];

         genf=(mi*xi-mj*xj);
         genf=genf+yj-yi;
         if (mi!=mj) genf=genf/(mi-mj);  // This is the X cross over point.
         else genf=32000;
         if (genf>20000)  VALID=0;  // invalid
         if (genf<-20000) VALID=0; // invalid
         if (genf>=0) X[i][j]=genf+0.5;
         else X[i][j]=genf-0.5;
         genf=mi*(genf-xi)+yi; // now calculate the Y
         if (genf>20000)  VALID=0;  // invalid
         if (genf<-20000) VALID=0; // invalid
         if (genf>=0) Y[i][j]=genf+0.5;
         else Y[i][j]=genf-0.5;

        // Now need to check that it is valid as far as the bearings are concerned
        // From each of the 2 points check that the cross position appears in the view of the bearing from those points.
        // For Bearings between 45 and 135 or 225 and 315 check the X position of the cross position is where expected
        // For other bearing check the Y position

        // consider i

        if (TB[TXNUM][i]>=45 && TB[TXNUM][i]<=135)  { if (X[i][j]<BSX[TXNUM][i]) VALID=0; }  // X cross must be bigger than start position
        if (TB[TXNUM][i]>=225 && TB[TXNUM][i]<=315) { if (X[i][j]>BSX[TXNUM][i]) VALID=0; }  // X cross must be smaller than start position
        if (TB[TXNUM][i]>315 && TB[TXNUM][i]<45)    { if (Y[i][j]<BSY[TXNUM][i]) VALID=0; }  // Y cross must be bigger than start position
        if (TB[TXNUM][i]>135 && TB[TXNUM][i]<225)   { if (Y[i][j]>BSY[TXNUM][i]) VALID=0; }  // Y cross must be smaller than start position
        // Now consider point j
        if (TB[TXNUM][j]>=45 && TB[TXNUM][j]<=135)  { if (X[i][j]<BSX[TXNUM][j]) VALID=0; }  // X cross must be bigger than start position
        if (TB[TXNUM][j]>=225 && TB[TXNUM][j]<=315) { if (X[i][j]>BSX[TXNUM][j]) VALID=0; }  // X cross must be smaller than start position
        if (TB[TXNUM][j]>315 && TB[TXNUM][j]<45)    { if (Y[i][j]<BSY[TXNUM][j]) VALID=0; }  // Y cross must be bigger than start position
        if (TB[TXNUM][j]>135 && TB[TXNUM][j]<225)   { if (Y[i][j]>BSY[TXNUM][j]) VALID=0; }  // Y cross must be smaller than start position
invalid:
        if (VALID==0)
        {   X[i][j]=32000;  // An error value
            Y[i][j]=32000;

        }
      }
   }


   if (NUMB[TXNUM]==2) // 2 bearings 1 cross, can't bubble through
   {  if (X[0][1]<32000 && Y[0][1]<32000)
      {  BESTX[TXNUM]=X[0][1];
         BESTY[TXNUM]=Y[0][1];
      }
      else
      {  BESTXYVALID[TXNUM]=0; // not valid
         BESTX[TXNUM]=32000;
         BESTY[TXNUM]=32000;
      }
      goto skipbest;
   }


   // Here we have between 3 and 8 bearings
   // now sort the best guess X and Y . Mode of the list we have obtained. x's & y's
   // 17/8/11 find cross points that include only the most recent line as this will be most accurate (assume getting closer and closer to the TX)
   // eg if we have 4 lines we need cross of 4 with 3, with2 and with 1. Previous we did 4 with 3,2,1. 3 with 2,1 and 2 with 1


   MAXXINDEX=255;
   MAXYINDEX=255;
   j= BEARINDEX[TXNUM]; // index of the most recent line
//   for(i=0;i<(NUMB[TXNUM]-1);i++)  // loop (bearings-1) times
   for(p=1;p<(NUMB[TXNUM]);p++) // loop around the other lines; generates NUMB[TXNUM]-1 loops. If 6 bearings p goes from 1 to 5, index of most recent line=5=i so j would go 4,3,2,1,0
   {  kk=j-p;
      if (kk<0) kk=kk+7; // loop around
      i=kk; // This is the index of the other line

//   {  for(j=(i+1);j<(NUMB[TXNUM]);j++)
      // j is defined as the most recent line
      {  if (X[i][j]<31800)  // Only add to the list if less than 31800
         {  MAXXINDEX++;
            SORTEDX[MAXXINDEX]=X[i][j];  // just get what we need
         }
         if (Y[i][j]<31800)
         {  MAXYINDEX++;
            SORTEDY[MAXYINDEX]=Y[i][j];  // just get what we need
         }
      }
   }
   // Here we have two lists SORTEDX and SORTEDY. Invalid points due to bearing direction have been removed. They are not sorted yet though!
   // Now sort the X and Y lists, SORTED[0] is highest.
bubblex:
   swap=0;
   for(k=1;k<(MAXXINDEX+1);k++) // for between 3 and 8 bearings. Sort into order SORTEDX[0] is the highest
   {  if (SORTEDX[k]>SORTEDX[k-1]) // swap around if true
      {  gens=SORTEDX[k-1];
         SORTEDX[k-1]=SORTEDX[k];
         SORTEDX[k]=gens;
         SWAP=1;
      }
    }
   if (SWAP==1) goto bubblex; // carry on until there have no more swaps
bubbley:
   swap=0;
   for(k=1;k<(MAXYINDEX+1);k++) // for between 2 and 8 bearings. Sort into order SORTEDY[0] is the highest
   {  if (SORTEDY[k]>SORTEDY[k-1]) // swap around if true
      {  gens=SORTEDY[k-1];
         SORTEDY[k-1]=SORTEDY[k];
         SORTEDY[k]=gens;
         SWAP=1;
      }
   }
   if (SWAP==1) goto bubbley;   // carry on until there have no more swaps


   // Now need to throw out any that are significantly far from the rest of the group
   // Find means of errors from the mean!
   // Do X first
   if (MAXXINDEX==255 || MAXYINDEX==255)
   {  BESTXYVALID[TXNUM]=0;
      goto skipbest;
   }
   // only here this if there is a valid list
   longint=0;
   for(k=0;k<(MAXXINDEX+1);k++) // Find mean
   {  longint=longint+SORTEDX[k];
   }
   longint=longint/(MAXXINDEX+1);  // find the mean
   gens=longint;  // convert to signed 16bit. This is the mean of the list
   // now find errors from mean
   longint=0;
   for(k=0;k<(MAXXINDEX+1);k++) // Find mean of errors
   {  longint=longint+abs(SORTEDX[k]-gens); //sum the absolute value of errors
   }
   merrs=longint/(MAXXINDEX+1);  // merrs=the mean of errors
   // Now check if any point is bigger than twice the mean away
   merrs=merrs<<1; // Twice the absolute value of the mean of errors
   // Max X index could now start reducing.
   for(k=0;k<(MAXXINDEX+1);k++) // Go through all X's
   {  errs=abs(SORTEDX[k]-gens);
      if (errs>merrs)  // need to remove this point and move everything upwards
      {  if (k==MAXXINDEX)
         {  MAXXINDEX--; // reduce the maximum index
            goto removedxdone;
         }
         for(i=k;i<MAXXINDEX;i++) // ripple through from present k to 1 less than the present maxindex
         {  SORTEDX[i]=SORTEDX[i+1]; // move everything up. We lose SORTED[k]
         }
         MAXXINDEX--; // reduce the amount in the list by 1
      }
   }
removedxdone:


   // now do the same for the Y's
   longint=0;
   for(k=0;k<(MAXYINDEX+1);k++) // Find mean
   {  longint=longint+SORTEDY[k];
   }
   longint=longint/(MAXYINDEX+1);  // find the mean
   gens=longint;  // convert to signed 16bit. This is the mean of the list
   // now find errors from mean
   longint=0;
   for(k=0;k<(MAXYINDEX+1);k++) // Find mean of errors
   {  longint=longint+abs(SORTEDY[k]-gens); //sum the absolute value of errors
   }
   merrs=longint/(MAXYINDEX+1);  // merrs=the mean of errors
   // Now check if any point is bigger than twice the mean away
   merrs=merrs<<1; // Twice the absolute value of the mean of errors
   // Max X index could now start reducing.
   for(k=0;k<(MAXYINDEX+1);k++) // Go through all Y's
   {  errs=abs(SORTEDY[k]-gens);
      if (errs>merrs)  // need to remove this point and move everything upwards
      {  if (k==MAXYINDEX)
         {  MAXYINDEX--; // reduce the maximum index
            goto removedydone;
         }
         for(i=k;i<MAXYINDEX;i++) // ripple through from present k to 1 less than the present maxindex
         {  SORTEDY[i]=SORTEDY[i+1]; // move everything up. We lose SORTED[k]
         }
         MAXYINDEX--; // reduce the amount in the list by 1
      }
   }
removedydone:

   // Now they are sorted and bad ones weeded out calculate best x and best y
   // X first
   if (MAXXINDEX & 0x01) // if odd
   {  gen=(MAXXINDEX-1); //minus 1
      gen=gen>>1; //divide by 2
      BESTX[TXNUM]=((SORTEDX[gen]+SORTEDX[gen+1])/2);
   }
   else  //if even
   {  gen=MAXXINDEX>>1; //divide by 2
      BESTX[TXNUM]=SORTEDX[gen];
   }
   // Now Y
   if (MAXYINDEX & 0x01) // if odd
   {  gen=(MAXYINDEX-1); //minus 1
      gen=gen>>1; //divide by 2
      BESTY[TXNUM]=((SORTEDY[gen]+SORTEDY[gen+1])/2);
   }
   else  //if even
   {  gen=MAXYINDEX>>1; //divide by 2
      BESTY[TXNUM]=SORTEDY[gen];
   }

skipbest:
   // To here takes approx 62mS from start of procedure (for 5 bearings)
   //LCD_PutCmd(1); // clear screen
   if (BESTX[TXNUM]>20000 || BESTY[TXNUM]>20000) BESTXYVALID[TXNUM]=0;
   //LCD_Clear();
   // To here takes approx 66mS
}

void get_currentxy()
{  // delta y=lat difference * radius of earth
   // delta x=long differnce * radius of earth * cos (latitude)
   CURRENTY=LATVAR*(LATITUDE-STARTLAT);  // LATVAR=6371000*PI/180
   CURRENTX=LONGVAR*(LONGITUDE-STARTLONG); // LONGVAR=LATVAR*cos(latitude)
}

void add_new_bearing()
{  unsigned int8 stindex;
   unsigned int16  err;
   int1 toosimilar;
   // Now need to check if we have the reference lat and long
   if (GOTREFXY==0)
   {  STARTLAT=LATITUDE;
      COSLAT=cos(STARTLAT*degtorad);
      STARTLONG=LONGITUDE;
      LONGVAR=LATVAR*COSLAT;
      GOTREFXY=1;     // We have reference
      CURRENTX=0.0; //
      CURRENTY=0.0;
      return;
   }
   // Need to check that we don't have any more lines of about the same bearing. The most recent voids the others
   if (NUMB[TXNUM]==0) goto skipthis; // no more lines to reference against.
   if (BEARINDEX[TXNUM]>0) stindex=(BEARINDEX[TXNUM]-1); // start from the most recent first
   else stindex=6;
chkbearings:
   toosimilar=0;
   err=abs(MB[TXNUM][stindex]-lastbearing);
   if (err<5) toosimilar=1; // 4 degrees or less
   if (err>355 && err<360) toosimilar=1;
   if (toosimilar)  // bearing difference wrt the most recent being less than 4 degrees
   {  IGNORE[TXNUM][stindex]=1;  // ignore this older bearing
   }
   if (stindex>0) stindex--;
   else stindex=6;
   if (stindex!=BEARINDEX[TXNUM]) goto chkbearings; // need to go through all the bearings first

skipthis:

   // Now add this most recent point
   IGNORE[TXNUM][BEARINDEX[TXNUM]]=0; // Reset the ignore flag
   NUMB[TXNUM]++;
   if (NUMB[TXNUM]>7) NUMB[TXNUM]=7; // Maximum of 7 bearings to any TX
   BSX[TXNUM][BEARINDEX[TXNUM]]=CURRENTX;
   BSY[TXNUM][BEARINDEX[TXNUM]]=CURRENTY;
   MB[TXNUM][BEARINDEX[TXNUM]]=lastbearing;
   get_bestxy();  // Now we have added a point find the best guess x&y for the particular transmitter
   BEARINDEX[TXNUM]++; // The next point to be added will have the next consequtive bearing index or roll over to 0 if at 6 already
   if (BEARINDEX[TXNUM]>6) BEARINDEX[TXNUM]=0; // cycle back to the start.

}

unsigned int8 convertint8(int8 start,int8 stop) //start and stop relate to the index of line1
{  int i=0;
   for(k=start;k<(stop+1);k++)
   {  gencha[i]=line1[k];
      i++;
   }
   gencha[i]=0;
   return ATOI(gencha);
}

unsigned int16 convertint16(int8 start,int8 stop) //start and stop relate to the index of line1
{  int i=0;
   for(k=start;k<(stop+1);k++)
   {  gencha[i]=line1[k];
      i++;
   }
   gencha[i]=0;
   return ATOL(gencha);
}

float convertft(int8 start,int8 stop) //start and stop relate to the index of line1
{  int i=0;
   float out;
   for(k=start;k<(stop+1);k++)
   {  gencha[i]=line1[k];
      i++;
   }
   gencha[i]=0;
   out=ATOF(gencha);
   return out;
}



void get_gps_info()
{  unsigned int16 TOTSECS;
   g=strstr(buffer,tofind2);  // address in RAM of the first character of tofind
   if (g==0)
   {  GPSSTATUS=0;
      return;
   }
   for(i=0;i<90;i++)
   {  if (*(g+i)==13) goto skipout; // carriage return
      line1[i]=*(g+i);   // make line1 string the next 90 characters starting at the find position
   }
 skipout:
      //line1[i]=0;
   HOURS=convertint8(4,5);  //
   MINUTES=convertint8(6,7);
   SECONDS=convertint8(8,9);
   if (GOTREFXY==0) // Havent yet got the reference
   {  TOTSECS=_MUL(60,MINUTES);  //convert minutes to secs
      TOTSECS=TOTSECS+SECONDS;
      reduce2:
      if (TOTSECS>=300)  // reduce to a single 5 minute section 0 to 299
      {  TOTSECS=TOTSECS-300;
         goto reduce2;
      }
      TIMEOFFSET=301-TOTSECS; // difference between competition time and GPS time
   }

      LATDEG=convertint8(17,18); //Assume always in the Northern Hemisphere
      GPSBEARING=convertint16(47,49);
      genf=convertft(19,25);  // convert to a float. Minutes
      genf=genf/60;
      genf2=LATDEG;
      LATITUDE=genf2+genf;
      LONGDEG=convertint8(29,31);
      genf=convertft(32,38);  // convert to a float. Minutes
      genf=genf/60;
      genf2=LONGDEG;
      LONGITUDE=genf2+genf;
      if (line1[40]==87) LONGITUDE=-LONGITUDE;  // if "W"
      if (line1[15]==65) GPSSTATUS=1; //if "A"
      else GPSSTATUS=0;

          // From skipout to here takes about 9mS
/* TEST *************************************************************************************
         LCD_Setposition(LINE_3 + 0 );
         if (line1[15]==65) // "A"  if A then fix is good, otherwise not
         {  printf ( LCD_PutChar,"Fix   %u:%u  ",MINUTES,SECONDS);
         }
         else
         {  printf ( LCD_PutChar,"Nofix %u:%u  ",MINUTES,SECONDS);
         }
         LCD_Setposition(LINE_3 + 19 );
         if (BATGOOD==0) printf ( LCD_PutChar,"B"); // Battery low indicator
         else printf ( LCD_PutChar," ");
*/

      // From skipout to here takes about 34mS

}

// If type=0 data required is raw data from the compass. Type=1, data is averaged and processed a bit
void  get_compass_bearing(unsigned int16 timeout, int1 type)  // check with a timeout nx10uS Compass sends every 200mS.
{  unsigned int16 quiet=0,br;
   unsigned int8 rschr,RS232INDEX,RS232_CHAR[17],rschar2,rschar3,rschar4;
   signed int16  err;
   int1 z,GOTSTRT=0,needtorotate=0;
   float B,error;
   bearingread=999; // default. Invalid
   GOTFROMCOMPASS=0; // Assume fail
   RS232INDEX=0;

recheck:
   z=kbhit(SPORT); // check for characters For a SW UART 1=start bit being sent
   if (z==0)
   {  delay_uS(4);
      quiet++;
      if (quiet>timeout)
      {  return; // timeout  after receiving nothing
      }
      goto recheck;
   }
   RS232_CHAR[RS232INDEX]=fgetc(SPORT);
   if (RS232_CHAR[RS232INDEX]==36)
   {  RS232INDEX=0;
      RS232_CHAR[0]=36;
      GOTSTRT=1;
   }
   RS232INDEX++;

   quiet=0;
waitmore:
   z=kbhit(SPORT); // check for characters For a SW UART 1=start bit being sent
   if (z==0)
   {  delay_uS(4);
      quiet++;
      if (quiet>500) return; // about 5mS of quiet
      goto waitmore;
   }
   RS232_CHAR[RS232INDEX]=fgetc(SPORT);

   if (RS232_CHAR[RS232INDEX]==42)
   {  if (GOTSTRT==0) return; // didn't get the 36
      rschar2=RS232_CHAR[2];
      rschar3=RS232_CHAR[3];
      rschar4=RS232_CHAR[4];
      GOTFROMCOMPASS=1;
      goto extractbearing;
   }
   quiet=0;
   RS232INDEX++;
   if (RS232INDEX>15) RS232INDEX=15;
   goto waitmore;
/*
   quiet=0;
check2:
   z=kbhit(SPORT); // check for characters For a SW UART 1=start bit being sent
   if (z==0)
   {  delay_uS(4);
      quiet++;
      if (quiet>1250) goto extractbearing; // timeout after 5mS after receiving something
      goto check2;
   }
   else
   {  chr=fgetc(SPORT);
      goto gotsomething;
   }
*/

extractbearing:

   if (RS232INDEX==5)
   {  br=(rschar2-48);
   }
   if (RS232INDEX==6) // double digit
   {  br=_MUL(10,(rschar2-48));
      br=br+(rschar3-48);
   }
   if (RS232INDEX==7) // triple digit
   {  br=_MUL(100,(rschar2-48));
      br=br+_MUL(10,(rschar3-48));
      br=br+(rschar4-48);
   }

   if (type==0) // raw data required
   {  B=br;
      if (B>=0) B=B+0.5; //due to errors in float to int conversion
      if (B<0) B=B-0.5;
      bearingread=B;
      bearingread=NORMBEARING(bearingread);
      return; // with raw data
   }
   // if here then need the processed data
   // Now average
   creading[compindex]=br;
   if (compindex<3) compindex++;
   else compindex=0;
   for(i=0;i<4;i++)
   {  if (creading[i]>300) needtorotate=1;  // if any value is above 300 degrees we may have problems with averaging around the 0,359 discontinuity
   }
   if (needtorotate) // if any value is above 300 degrees we may have problems with averaging around the 0,359 discontinuity
   {  for(i=0;i<4;i++)
      {  creading[i]=creading[i]-180;
         creading[i]=NORMBEARING(creading[i]);
      }
   }
   // now average.
   g1=0;
   for(i=0;i<4;i++)  {  g1=g1+creading[i]; }
   B=g1/4.0;   // find mean
   if (needtorotate)   // Now need to rotate back
   {  for(i=0;i<4;i++)
      {  creading[i]=creading[i]+180;
         creading[i]=NORMBEARING(creading[i]);
      }
      B=B+180;
      if (B>=360) B=B-360;
   }
   // Now sort out errors. see digital.compass_error.xls


 //  B=br; //put this line in if the above average is removed
   //B=raw measured data
   error=Aerror*cos((B+Perror)*degtorad);
   B=B-error+COMPASS_OFFSET; // add together all the factors
   if (B>=0) B=B+0.5; //due to errors in float to int conversion
   if (B<0) B=B-0.5;
   bearingread=B;
   bearingread=NORMBEARING(bearingread);
   NUMCOMPREADINGS++;

}

void findTXperiod()  //The output is TXNUM. There are periods that
{  unsigned int16 TOTSECS;
   TOTSECS=_MUL(60,MINUTES);  //convert minutes to secs
   TOTSECS=TOTSECS+SECONDS+TIMEOFFSET;  // include the time offset between GPS and competition time
reduce:
   if (TOTSECS>=300)  // reduce to a single 5 minute section 0 to 299
   {  TOTSECS=TOTSECS-300;
      goto reduce;
   }
   ALLOW=0;
   // Overlap is such that we count a trigger press of up to 2 seconds beyond the timing threshold as the previous TX
   // We do not allow a press for 2 seconds following this
   if (MODE==1)  // Only one transmitter. Always TX1 index=0
   {  TXNUM=0;
      ALLOW=1;
      MAXTX=1;
   }
   if (MODE==5)  // 5 TX's 1minute each
   {  ALLOW=1; // Assume OK
      if (TOTSECS<=2) {TXNUM=4;}
      if (TOTSECS>2 && TOTSECS<6) {ALLOW=0;}
      if (TOTSECS>=6 && TOTSECS<=62) {TXNUM=0;}
      if (TOTSECS>62 && TOTSECS<66) {ALLOW=0;}
      if (TOTSECS>=66 && TOTSECS<=122) {TXNUM=1;}
      if (TOTSECS>122 && TOTSECS<126) {ALLOW=0;}
      if (TOTSECS>=126 && TOTSECS<=182) {TXNUM=2;}
      if (TOTSECS>182 && TOTSECS<186) {ALLOW=0;}
      if (TOTSECS>=186 && TOTSECS<=242) {TXNUM=3;}
      if (TOTSECS>242 && TOTSECS<246) {ALLOW=0;}
      if (TOTSECS>=246 && TOTSECS<=299) {TXNUM=4;}
      MAXTX=5;
   }
   if (MODE==7)  // 7 TX's 43 secs each
   {  ALLOW=1; // Assume OK
      if (TOTSECS<=2) {TXNUM=6;}
      if (TOTSECS>2 && TOTSECS<6) {ALLOW=0;}
      if (TOTSECS>=6 && TOTSECS<=45) {TXNUM=0;}
      if (TOTSECS>45 && TOTSECS<49) {ALLOW=0;}
      if (TOTSECS>=49 && TOTSECS<=88) {TXNUM=1;}
      if (TOTSECS>88 && TOTSECS<92) {ALLOW=0;}
      if (TOTSECS>=92 && TOTSECS<=131) {TXNUM=2;}
      if (TOTSECS>131 && TOTSECS<135) {ALLOW=0;}
      if (TOTSECS>=135 && TOTSECS<=174) {TXNUM=3;}
      if (TOTSECS>174 && TOTSECS<178) {ALLOW=0;}
      if (TOTSECS>=178 && TOTSECS<=217) {TXNUM=4;}
      if (TOTSECS>217 && TOTSECS<221) {ALLOW=0;}
      if (TOTSECS>=221 && TOTSECS<=260) {TXNUM=5;}
      if (TOTSECS>260 && TOTSECS<264) {ALLOW=0;}
      if (TOTSECS>=264 && TOTSECS<=299) {TXNUM=6;}
      MAXTX=7;
   }
   if (MODE==8)  // 8 TX's 37.5 secs each
   {  ALLOW=1; // Assume OK
      if (TOTSECS<=2) {TXNUM=7;}
      if (TOTSECS>2 && TOTSECS<6) {ALLOW=0;}
      if (TOTSECS>=6 && TOTSECS<=39) {TXNUM=0;}
      if (TOTSECS>39 && TOTSECS<43) {ALLOW=0;}
      if (TOTSECS>=43 && TOTSECS<=77) {TXNUM=1;}
      if (TOTSECS>77 && TOTSECS<81) {ALLOW=0;}
      if (TOTSECS>=81 && TOTSECS<=114) {TXNUM=2;}
      if (TOTSECS>114 && TOTSECS<118) {ALLOW=0;}
      if (TOTSECS>=118 && TOTSECS<=152) {TXNUM=3;}
      if (TOTSECS>152 && TOTSECS<156) {ALLOW=0;}
      if (TOTSECS>=156 && TOTSECS<=189) {TXNUM=4;}
      if (TOTSECS>189 && TOTSECS<193) {ALLOW=0;}
      if (TOTSECS>=193 && TOTSECS<=227) {TXNUM=5;}
      if (TOTSECS>227 && TOTSECS<231) {ALLOW=0;}
      if (TOTSECS>=231 && TOTSECS<=264) {TXNUM=6;}
      if (TOTSECS>264 && TOTSECS<268) {ALLOW=0;}
      if (TOTSECS>=268 && TOTSECS<=299) {TXNUM=7;}
      MAXTX=8;
   }
}

void configuration()
{  unsigned int8 i,k,CNT, check,TESTBEARINDEX,A;
   unsigned int16 TESTBEAR,P;
   signed int16 errbear[8],MEASBEAR;
   float ESTERR,RECONSBEAR,errrecon,errtot,errmin;
   int1 justmeas=0;
   // read the variables from eeprom
   check=read_eeprom(1); // should be 55 if we have previously been here
   if (check!=55) goto set;
   MODE=read_eeprom(10);
   COMPASS_OFFSET=read_eeprom(11);
   MAG_VARIATION=read_eeprom(12);
   MAPSCALE=read_eeprom(13);
   Aerror=read_eeprom(14);
   i=read_eeprom(15);
   k=read_eeprom(16);
   Perror=make16(i,k);
   if (input(TRIGGER)==1) return;
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Configuration Mode");
   LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release SW");
cnf:
   delay_ms(100);
   if (input(TRIGGER)==0) goto cnf;

set:
// now setup the Mode
   delay_ms(100);
   if (MODE!=1 && MODE!=5 && MODE!=7 && MODE!=8) MODE=1;
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"  MODE    ");
   LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Press to setup");
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
cnf1:
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnf1;
newmode:
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"MODE=%i", MODE);
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
   CNT=0;
cnf2:
   CNT++;
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnf3;
   if (CNT>20)
   {  if (MODE==1) {MODE=5; goto  newmode;}
      if (MODE==5) {MODE=7; goto  newmode;}
      if (MODE==7) {MODE=8; goto  newmode;}
      if (MODE==8) {MODE=1; goto  newmode;}
      goto  newmode; // Wrap round a
   }
   goto cnf2;
cnf3:
   write_eeprom(10,MODE);


// now setup the Compass Offset
   delay_ms(100);
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar," Compass Offset");
   LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Press to setup");
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
cnfc1:
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfc1;
newcomp:
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Offset=%i", COMPASS_OFFSET);
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
   CNT=0;
cnfc2:
   CNT++;
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfc3;
   if (CNT>20)
   {  COMPASS_OFFSET++;
      if (COMPASS_OFFSET>20) COMPASS_OFFSET=-20;
      goto  newcomp; // Wrap round a
   }
   goto cnfc2;
cnfc3:
   write_eeprom(11,COMPASS_OFFSET);

// now setup the Magnetic Variation
   delay_ms(100);
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar," Magnetic Variation");
   LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Press to setup");
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
cnfm1:
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfm1;
newmag:
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Mag Variation=%i", MAG_VARIATION);
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
   CNT=0;
cnfm2:
   CNT++;
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfm3;
   if (CNT>10)
   {  MAG_VARIATION++;
      if (MAG_VARIATION>10) MAG_VARIATION=-10;
      goto  newmag; // Wrap round a
   }
   goto cnfm2;
cnfm3:
   write_eeprom(12,MAG_VARIATION);


// now setup the Map Scale
   delay_ms(100);
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar," Map Scale         ");
   LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Press to setup");
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
cnfms1:
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfms1;
newscale:
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Map Scale=%u00  ", MAPSCALE);
   LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Release to store");
   CNT=0;
cnfms2:
   CNT++;
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfms3;
   if (CNT>20)
   {  MAPSCALE=MAPSCALE+25;
      if (MAPSCALE>200) MAPSCALE=25;
      goto  newscale; // Wrap round a
   }
   goto cnfms2;
cnfms3:
   write_eeprom(13,MAPSCALE);



// now setup the Compass
   delay_ms(100);
   LCD_Clear();                // clear screen
   LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar," Compass           ");
   LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
   printf ( LCD_PutChar,"Hold to setup");
cnfmk1:
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfmk1; // wait until trigger is pressed again
   CNT=0;
cnfmk2:
   CNT++;
   delay_ms(100);
   if (input(TRIGGER)==1) goto cnfmk3; // trigger released within 2 secs
   if (CNT>20) // enter the setup routine
   {  LCD_Clear();                // clear screen
      LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar," Compass  Setup");
      LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Now release");
cnfmk4:
      if (input(TRIGGER)==0) goto cnfmk4; // await trigger release 
      delay_ms(100);
      TESTBEAR=0;
      TESTBEARINDEX=0;
      LCD_Clear();                // clear screen

getnxtcomp:
      //delay_ms(100);
      if (input(TRIGGER)==0 && justmeas==1) goto getnxtcomp; // only advance if trigger is released
      justmeas=0;
      LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Rotate to %lu deg  ",TESTBEAR);
      LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Press when done");
      get_compass_bearing(800,0); // await compass bearing info for 20mS
      if (bearingread<360)
      {  LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
         printf ( LCD_PutChar,"Compass=%lu  ",bearingread);
         if (input(TRIGGER)==0)
         {  justmeas=1;
            errbear[TESTBEARINDEX]=bearingread-TESTBEAR;
            if (errbear[TESTBEARINDEX]>180) errbear[TESTBEARINDEX]=errbear[TESTBEARINDEX]-360;
            TESTBEAR=TESTBEAR+45;
            TESTBEARINDEX++;
            if (TESTBEAR==360) goto sorterrors;
            delay_ms(100);
         }
      }
/*      
      //test
      errbear[0]=-5;  //0 deg
      errbear[1]=-2;  //45
      errbear[2]=0;  //90
      errbear[3]=3;  //135
      errbear[4]=5;  //180
      errbear[5]=3;  //225
      errbear[6]=0;  //270
      errbear[7]=-3;  //315
      goto sorterrors;
      //test
*/      
      
      goto getnxtcomp;
sorterrors:
      // if here then have an array of errors at 0,45,90,135,180,225,270 and 315 degrees
      // Assume error function takes the form Aerror * Sin(measured angle+Perror) Aerror=amplitude of error. Perror=phase of error.
      // Need to find Aerror and Perror. Ripple through them to find minimum error.
      errmin=32000; // set it high to begin with
      LCD_Clear();                // clear screen
      for(A=0;A<16;A++) // itterate through the amplitudes
      {  
         LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
         printf ( LCD_PutChar,"Calculating ...");
         LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
         printf ( LCD_PutChar,"Try amp=%u  ",A);
         P=0;
         for(i=0;i<72;i++) // ultimately step phase 0 to 355 step 5
         {  errtot=0;
            TESTBEAR=0; // START AT 0 AND STEP IN 45
            for(TESTBEARINDEX=0;TESTBEARINDEX<8;TESTBEARINDEX++) // look at all the errors
            {  MEASBEAR=TESTBEAR+errbear[TESTBEARINDEX];
               ESTERR=A*cos((MEASBEAR+P)*degtorad);
               RECONSBEAR=MEASBEAR; // TURN TO A FLOAT
               RECONSBEAR=(RECONSBEAR-ESTERR);
               errrecon=RECONSBEAR-TESTBEAR;
               errtot=errtot+abs(errrecon);
               TESTBEAR=TESTBEAR+45;
            }
            if (errtot<errmin)
            {  errmin=errtot;
               Aerror=A;
               Perror=P;
            }
            P=P+5;
         }
         LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
         printf ( LCD_PutChar,"Min error =%3.2f  ",errmin);



      }
      LCD_Clear();                // clear screen
      LCD_SetPosition ( LINE_1 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Done" ) ;
      LCD_SetPosition ( LINE_2 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Final amp=%u  ",Aerror);
      LCD_SetPosition ( LINE_3 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Final phase=%lu  ",Perror);
      LCD_SetPosition ( LINE_4 + 0 );                   // set line and offset on line
      printf ( LCD_PutChar,"Error min=%f  ",errmin);
      delay_ms(2000);

      
      // here should have optimum Aerror and Perror
      write_eeprom(14,Aerror);
      i=make8(Perror,1);
      write_eeprom(15,i);
      i=make8(Perror,0);
      write_eeprom(16,i);

   } // if cnt>10
   else  goto cnfmk2; // either wait for 2 secs with trigger held and go into setup or loose trigger to continue

cnfmk3:

   write_eeprom(1,55); // check byte. Config done
   delay_ms(1000);
}

// write a float to eeprom. addr is the start address of the float variable, ad is the eeprom start address. 4 bytes
void write_eepf(int16 addr,int8 ad)
{  unsigned int8 data,i;
   unsigned int16 XX;
   for(i=0;i<4;i++)
   {  XX=ad+i;                         // address in eeprom to write byte to.
      data=*(addr+i);                 // data is the contents of the address
      write_eeprom(XX,data);
   }
}


void DISPLAY_TRACK_ERROR()
{    get_bearing_distance(startx,starty,CURRENTX,CURRENTY); // get the Magnetic bearing startx and starty points to the current point
     TRACKERROR=MAGBEARING-bearingtofollow;
     if (TRACKERROR>180) TRACKERROR=TRACKERROR-360;
     if (TRACKERROR<-180) TRACKERROR=TRACKERROR+360;
     genf=degtorad*TRACKERROR*DISTANCE; // estimate for small angles
     DISTANCEERROR=genf;
     DISTANCEERROR=abs(DISTANCEERROR);
     LCD_Setposition(LINE_4 + 6 );
     if (TRACKERROR<0) printf ( LCD_PutChar,">%lim%lu",DISTANCEERROR,bearingtofollow);
     else printf ( LCD_PutChar,             "<%lim%lu",DISTANCEERROR,bearingtofollow);
     LCD_PutChar(0xDF); // degree symbol
     printf ( LCD_PutChar,"   ");
//     printf ( LCD_PutChar," %lu ",NUMCOMPREADINGS);
     NUMCOMPREADINGS=0;
}


// Display three pixels plus the centre pixel
void DISPLAY_MAP()
{  // Consider each character as we print it
   unsigned int8 chrrowstart,chrcolstart,chrrowend,chrcolend,chrx,chry,i,j,k,m,nospec,spr[4],spc[4],r3;
   int1 pix,pixactive;

recalcc:
   gens=(BESTY[MAPTXNUM]-CURRENTY)/STEP[MAPTXNUM];
   if (gens<-15) gens=-15;
   if (gens>16) gens=16; // max ROW=32
   PROW=16+gens; // 1 to 32 mid row=16
   if (PROW==1 || PROW==32)
   {  STEP[MAPTXNUM]=STEP[MAPTXNUM]<<1; // X2 zoom out
      ROW[1]=0;ROW[2]=0;ROW[3]=0;COL[1]=0;COL[2]=0;COL[3]=0;
      goto recalcc;
   }
   gens=(CURRENTX-BESTX[MAPTXNUM])/STEP[MAPTXNUM];
   if (gens<-14) gens=-14;
   if (gens>15) gens=15; // Max COL=30 (6X5)
   PCOL=15+gens; // 1 to 30 mid column=15
   if (PCOL==1 || PCOL==30)
   {  STEP[MAPTXNUM]=STEP[MAPTXNUM]<<1; // X2 zoom out
      ROW[1]=0;ROW[2]=0;ROW[3]=0;COL[1]=0;COL[2]=0;COL[3]=0;
      goto recalcc;
   }
   if (PCOL>11 && PCOL<19 && PROW>12 && PROW<20 && STEP[MAPTXNUM]>7)  // do not zoom in more than 4m step
   {  if (STEP[MAPTXNUM]>15) STEP[MAPTXNUM]=STEP[MAPTXNUM]>>2; // /4 zoom in TWICE
      else STEP[MAPTXNUM]=STEP[MAPTXNUM]>>1; // Zoom in only once
      ROW[1]=0;ROW[2]=0;ROW[3]=0;COL[1]=0;COL[2]=0;COL[3]=0;
      goto recalcc;
   }

   if (PCOL!=COL[3] || PROW!=ROW[3]) // if the latest info is different then update it
   {  for(i=1;i<3;i++)  // Ripple up
      {  ROW[i]=ROW[i+1];
         COL[i]=COL[i+1];
      }
      COL[3]=PCOL;
      ROW[3]=PROW;
   }

   r3=ROW[3];
   if (PHASEDIS) ROW[3]=0; // Make the last point flash
   ROW[0]=16;
   COL[0]=15; // centre point
   chrrowstart=1;
   nospec=0;
   lcd_PutCmd(64); // Sets ram address to beginning of CGRAM area

   // add the special character for the change direction character
   for(i=1;i<5;i++)  // Ripple up
   {  OLDX[i]=OLDX[i+1];
      OLDY[i]=OLDY[i+1];
   }
   OLDX[5]=CURRENTX;
   OLDY[5]=CURRENTY;

   get_bearing_distance(OLDX[1],OLDY[1],CURRENTX,CURRENTY); // get the Magnetic bearing from point 5 seconds ago to current point
   BEARING_LAST_5SEC=MAGBEARING;
   get_bearing_distance(CURRENTX,CURRENTY,BESTX[MAPTXNUM],BESTY[MAPTXNUM]); // get the Magnetic bearing from current point to best guess point
   ERRORINBEARING=MAGBEARING-BEARING_LAST_5SEC;
   if (ERRORINBEARING>180) ERRORINBEARING=ERRORINBEARING-360;
   if (ERRORINBEARING<-180) ERRORINBEARING=ERRORINBEARING+360;
   // here ERRORINBEARING ranges from -180 to +180
   if (ERRORINBEARING>=-11 && ERRORINBEARING<11) { lin1=0x04; lin2=0x0A; lin3=0x15; lin4=0x04; lin5=0x04; lin6=0x04; lin7=0x04; lin8=0x04; goto chrdone; }
   if (ERRORINBEARING>=11 && ERRORINBEARING<34) { lin1=0x04; lin2=0x0A; lin3=0x15; lin4=0x04; lin5=0x08; lin6=0x08; lin7=0x10; lin8=0x10; goto chrdone; }
   if (ERRORINBEARING>=34 && ERRORINBEARING<56) { lin1=0x0F; lin2=0x03; lin3=0x05; lin4=0x09; lin5=0x10; lin6=0x00; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=56 && ERRORINBEARING<79) { lin1=0x00; lin2=0x02; lin3=0x01; lin4=0x07; lin5=0x19; lin6=0x02; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=79 && ERRORINBEARING<101) { lin1=0x00; lin2=0x04; lin3=0x02; lin4=0x1d; lin5=0x02; lin6=0x04; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=101 && ERRORINBEARING<124) { lin1=0x00; lin2=0x02; lin3=0x19; lin4=0x07; lin5=0x01; lin6=0x02; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=124 && ERRORINBEARING<146) { lin1=0x00; lin2=0x00; lin3=0x00; lin4=0x10; lin5=0x09; lin6=0x05; lin7=0x03; lin8=0x0F; goto chrdone; }
   if (ERRORINBEARING>=146 && ERRORINBEARING<169) { lin1=0x10; lin2=0x10; lin3=0x08; lin4=0x08; lin5=0x04; lin6=0x15; lin7=0x0A; lin8=0x04; goto chrdone; }
   if (ERRORINBEARING>=169 || ERRORINBEARING<-169) { lin1=0x04; lin2=0x04; lin3=0x04; lin4=0x04; lin5=0x04; lin6=0x15; lin7=0x0A; lin8=0x04; goto chrdone; }
   if (ERRORINBEARING>=-169 && ERRORINBEARING<-146) { lin1=0x01; lin2=0x01; lin3=0x02; lin4=0x02; lin5=0x04; lin6=0x15; lin7=0x0A; lin8=0x04; goto chrdone; }
   if (ERRORINBEARING>=-146 && ERRORINBEARING<-124) { lin1=0x00; lin2=0x00; lin3=0x00; lin4=0x01; lin5=0x12; lin6=0x14; lin7=0x18; lin8=0x1E; goto chrdone; }
   if (ERRORINBEARING>=-124 && ERRORINBEARING<-101) { lin1=0x00; lin2=0x08; lin3=0x13; lin4=0x1C; lin5=0x10; lin6=0x08; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=-101 && ERRORINBEARING<-79) { lin1=0x00; lin2=0x04; lin3=0x08; lin4=0x17; lin5=0x08; lin6=0x04; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=-79 && ERRORINBEARING<-56) { lin1=0x00; lin2=0x08; lin3=0x10; lin4=0x1C; lin5=0x13; lin6=0x08; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=-56 && ERRORINBEARING<-34) { lin1=0x1E; lin2=0x18; lin3=0x14; lin4=0x12; lin5=0x01; lin6=0x00; lin7=0x00; lin8=0x00; goto chrdone; }
   if (ERRORINBEARING>=-34 && ERRORINBEARING<-11) { lin1=0x04; lin2=0x0A; lin3=0x15; lin4=0x04; lin5=0x02; lin6=0x02; lin7=0x01; lin8=0x01; goto chrdone; }

chrdone:
   lcd_PutChar(lin1);
   lcd_PutChar(lin2);
   lcd_PutChar(lin3);
   lcd_PutChar(lin4);
   lcd_PutChar(lin5);
   lcd_PutChar(lin6);
   lcd_PutChar(lin7);
   lcd_PutChar(lin8);

   // generate the map special characters
   for(j=1;j<5;j++) // j goes from 1 to 4. y posn
   {  chrcolstart=1;
      for(i=1;i<7;i++) // i goes from 1 to 6. x posn
      {  chrrowend=chrrowstart+8;  // actually 1 more than the end posn
         chrcolend=chrcolstart+5;
         pix=0; // assume no pixel
         for(k=0;k<4;k++) // see if this character contains any pixels
         {  if (ROW[k]>=chrrowstart && ROW[k]<chrrowend && COL[k]>=chrcolstart && COL[k]<chrcolend) pix=1; // a pixel is in this character
         }
         if (pix==1)
         {  spr[nospec]=i;  // special chr row=i
            spc[nospec]=j; // special chr column=j
            nospec++; // one more special character
            for(chry=chrrowstart;chry<chrrowend;chry++) // go through all rows of the character
            {  linedefine=0;
               for(chrx=chrcolstart;chrx<chrcolend;chrx++) // go through all colums of the character
               {  pixactive=0; // assume not
                  for(k=0;k<4;k++) // go through all the points
                  {  if (ROW[k]==chry && COL[k]==chrx) pixactive=1; // this pixel is on
                  }
                  if (pixactive)
                  {  linedefine=linedefine+(1<<(4-(chrx-chrcolstart)));
                  }
               }
               lcd_PutChar(linedefine);
            }
         }
         chrcolstart=chrcolstart+5;
      } // next i
      chrrowstart=chrrowstart+8;
   } // next j


   // now print them
   nospec=0;
   for(j=1;j<5;j++) // j goes from 1 to 4. y posn row
   {  for(i=1;i<7;i++) // i goes from 1 to 6. x posn col
      {  if (j==1) LCD_Setposition(LINE_1 + (i-1) );
         if (j==2) LCD_Setposition(LINE_2 + (i-1) );
         if (j==3) LCD_Setposition(LINE_3 + (i-1) );
         if (j==4) LCD_Setposition(LINE_4 + (i-1) );

         if (spr[nospec]==i && spc[nospec]==j)
         {  LCD_PutChar(nospec+1); // print the special character
            nospec++; // go on to the next special character
         }
         else LCD_PutChar(32); // make it blank
      }
   }

   ROW[3]=r3; // revert back

   // sort out line 3
   LCD_Setposition(LINE_3 + 6 );
   printf ( LCD_PutChar,"              ");
   LCD_Setposition(LINE_3 + 6 );
   LCD_PutChar(0); // print the 1st special character
   printf ( LCD_PutChar,"TX%u ",MAPTXNUM+1);
   printf ( LCD_PutChar,"%lim%li",DISTANCE,MAGBEARING);
   LCD_PutChar(0xDF); // degree symbol
   // sort out line 4
   DISPLAY_TRACK_ERROR();  // this goes on line 4
   // sort out line 2
   genf=DISTANCE;
   maplength=genf/MAPSCALE;
   LCD_Setposition(LINE_2 + 6 );
   printf ( LCD_PutChar,"Map=%3.1fcm  ",maplength);
//   printf ( LCD_PutChar,"Step=%lum   ",STEP[MAPTXNUM]);


}

void DISPLAY_TEXT()
{
   LCD_Clear();
   // Sort out lines 2,3 and 4
   CLOSESTDISTANCE=32000;
   if (MAXTX>5)
   {  for(i=0;i<MAXTX;i++)
      {  if (i==0) LCD_Setposition(LINE_2 + 0 );
         if (i==1) LCD_Setposition(LINE_2 + 7 );
         if (i==2) LCD_Setposition(LINE_2 + 14 );
         if (i==3) LCD_Setposition(LINE_3 + 0 );
         if (i==4) LCD_Setposition(LINE_3 + 7 );
         if (i==5) LCD_Setposition(LINE_3 + 14 );
         if (i==6) LCD_Setposition(LINE_4 + 0 );
         if (i==7) LCD_Setposition(LINE_4 + 7 );

         if (BESTXYVALID[i]==1) // valid guess posn
         {  get_bearing_distance(CURRENTX,CURRENTY,BESTX[i],BESTY[i]); // get the Magnetic bearing from current point to best guess point
            if (DISTANCE<CLOSESTDISTANCE)
            {  CLOSESTDISTANCE=DISTANCE;
               CLOSESTMAGBEARING=MAGBEARING;
               CLOSESTTX=i+1;
            }
            if (DISTANCE>999) DISTANCE=999;
            DISTDIVTEN=(DISTANCE+5)/10;  // To make the threshold halfway.
            if (DISTDIVTEN>99) DISTDIVTEN=99;
            printf ( LCD_PutChar,"%lid%li",DISTDIVTEN,MAGBEARING); // if 5 or less transmitters show the 1/10 distance. Not enough space on the display

         }
         else // haven't got a valid guess position for this TX
         {  printf ( LCD_PutChar,"%uX",i+1);

         }
      } // end of for loop
      if (PHASEDIS || MAXTX<7)  // display either the error from bearing or distance to TX7 and 8
      {  DISPLAY_TRACK_ERROR(); // display the distance and direction back to the bearing you should be on
      }
   }
   else // MAXTX is 5 or less
      {  for(i=0;i<MAXTX;i++)
      {  if (i==0) LCD_Setposition(LINE_2 + 0 );
         if (i==1) LCD_Setposition(LINE_2 + 10 );
         if (i==2) LCD_Setposition(LINE_3 + 0 );
         if (i==3) LCD_Setposition(LINE_3 + 10 );
         if (i==4) LCD_Setposition(LINE_4 + 6 );

         if (BESTXYVALID[i]==1) // valid guess posn
         {  get_bearing_distance(CURRENTX,CURRENTY,BESTX[i],BESTY[i]); // get the Magnetic bearing from current point to best guess point
            if (DISTANCE<CLOSESTDISTANCE)
            {  CLOSESTDISTANCE=DISTANCE;
               CLOSESTMAGBEARING=MAGBEARING;
               CLOSESTTX=i+1;
            }
            if (DISTANCE>999) DISTANCE=999;
           // DISTDIVTEN=(DISTANCE+5)/10;  // To make the threshold halfway.
           // if (DISTDIVTEN>99) DISTDIVTEN=99;
           // printf ( LCD_PutChar,"%li,%li",DISTANCE,MAGBEARING); // if 5 or less transmitters show the proper distance

            printf ( LCD_PutChar,"%lim%li",DISTANCE,MAGBEARING); // if 5 or less transmitters show the proper distance
            LCD_PutChar(0xDF); // degree symbol

         }
         else // haven't got a valid guess position for this TX
         {  printf ( LCD_PutChar,"%uX",i+1);
         }
      } // end of for loop
      if (PHASEDIS || MAXTX<5)  // display either the error from bearing or distance to TX7 and 8
      {  DISPLAY_TRACK_ERROR(); // display the distance and direction back to the bearing you should be on
      }
   }


/*
   j=0;
   for(k=0;k<(MAXXINDEX+1);k++) //
   {  if (k<4)
      {  LCD_Setposition(LINE_2 + j );
         j=j+5;
         if (PHASEDIS) printf ( LCD_PutChar,"%li,",SORTEDX[k]);
         else printf ( LCD_PutChar,"%li,",SORTEDY[k]);
      }
      else
      {  if (k==4) {j=0;}
         LCD_Setposition(LINE_3 + j );
         j=j+5;
         if (PHASEDIS) printf ( LCD_PutChar,"%li,",SORTEDX[k]);
         else printf ( LCD_PutChar,"%li,",SORTEDY[k]);
      }
   }

   LCD_Setposition(LINE_4 + 0 );
   printf ( LCD_PutChar,"%li %li      ",CURRENTX,CURRENTY);

*/



            // TEST *********************************************************************************************
/*
            LCD_Setposition(LINE_4 + 0 );
            if (BESTXYVALID[TXNUM]==1)
            {  get_bearing_distance(CURRENTX,CURRENTY,BESTX[TXNUM],BESTY[TXNUM]); // get the Magnetic bearing from current point to best guess point
               printf ( LCD_PutChar,"DIST=%li MBEAR=%li ",DISTANCE,MAGBEARING);
               // The printing takes about 2mS
            }
            else printf ( LCD_PutChar,"                    ");
*/

            // test ****************************************

//            LCD_Setposition(LINE_3 + 0 );
//            if (BESTXYVALID[TXNUM]==1)
//            {  printf ( LCD_PutChar,"%li %li  %li %li  ",BESTX[TXNUM],BESTY[TXNUM],CURRENTX,CURRENTY);
//            }
//            else printf( LCD_PutChar,"No best             ");

}

void checkmapsw()  // goto map display if the map switch changes
{  unsigned int8 gen;
   gen=input_b();  // use this to read the side switch
   gen=gen & 7;
   if (gen!=OLDMAPTX)
   {  DISPMODE=1; // A change in the switch position causes the map to be displayed
      ROW[1]=0;ROW[2]=0;ROW[3]=0;COL[1]=0;COL[2]=0;COL[3]=0;  // get rid of old dots
   }
   OLDMAPTX=gen;
   if (gen==0) MAPTXNUM=7; // SW is pointing to number 8
   else MAPTXNUM=gen-1;
}

// ******************************************* main *****************************************


void main()
{

strt:
//   enable_interrupts(GLOBAL);
//   enable_interrupts(INT_RDA);
   setup();
   set_adc_channel(0); // set it for reading the battery voltage
   EXTENDEDPUSH=0;
   GOTREFXY=0;
   configuration();

   lcd_PutCmd(64); // Sets ram address to beginning of CGRAM area
   for (i=0; i<15;i++)  // fill in CGRAM with the data. this does two characters, 0 and 1. 0 is blank, 1 has one pixel.
      lcd_PutChar(0);  // first character is blank, this is set later. Second character is blank apart dot in bottom right
   lcd_PutChar(1); // sets the dot for the centre point in the 8th row 5th column. Relates to overall ROW=16 COL=15

askagn:
   if (!input(TRIGGER))
   {  LCD_Setposition(LINE_1 + 0 );
      printf ( LCD_PutChar,"Release Button");
      delay_ms(200);
      goto askagn;

   }


   for(i=0;i<8;i++) //setup the initial values to be invalid
   {  BESTXYVALID[i]=0;
      STEP[i]=32;
      for(j=0;j<7;j++)
      {   MB[i][j]=999; //invalid
      }
   }


   TXNUM=0; // 0 to 4 for 5 TX's
   NUMB[TXNUM]=0;  // we have 6 bearings to TX1
   BEARINDEX[TXNUM]=0; // This will be the next bearing index

   if (read_eeprom(100)==56)   // if we have reset from an extended push, keep the pre-set origin
   {  addr=&STARTLAT;                    // address of variable
      data=read_eeprom(101);
      *addr=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(102);
      *(addr+1)=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(103);
      *(addr+2)=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(104);
      *(addr+3)=data;                      // contents of the address = data read from eeprom
      addr=&STARTLONG;                    // address of variable
      data=read_eeprom(105);
      *addr=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(106);
      *(addr+1)=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(107);
      *(addr+2)=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(108);
      *(addr+3)=data;                      // contents of the address = data read from eeprom
      addr=&LONGVAR;                    // address of variable
      data=read_eeprom(109);
      *addr=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(110);
      *(addr+1)=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(111);
      *(addr+2)=data;                      // contents of the address = data read from eeprom
      data=read_eeprom(112);

      GOTREFXY=1;     // We have reference
      write_eeprom(100,0);             // don't keep on doing this whenever we reset
      delay_ms(100);
   }

   LCD_Clear();
   LCD_Setposition(LINE_1 + 0 );
   printf ( LCD_PutChar," Awaiting GPS signal");



stloop1:              // get here every 100ms


//   g=get_timer0();
//   if (g<777) { goto stloop1; }
//   set_timer0(0);
//   restart_WDT();

   //phase++;

   gflag=kbhit(GPS);  // =1 if something in the GPS character buffer
   if (gflag==0)  // nothing from the GPS
   {  if (SWFLAG==0) // Do not update the compass reading while SWFLAG=1 as there is a tendency to move the equipment just after the button is pressed)
      {  get_compass_bearing(800,1); // await compass bearing info for 20mS (Can afford to miss the first bit from the GPS as I await the GPRMC command)
         if (bearingread<360)
         {  lastbearing=bearingread; // valid. Now need to add this as another point
         }

      }
      if (GOTFROMCOMPASS || SWFLAG) // if GOTFROMCOMPASS then I've just got a bearing from the compass so I can relax a bit or do the routine anyway if SWFLAG=1 as I dont update the compass reading.
      {      // put here commands that need to be polled that maybe takes a bit more time.
         battery = read_adc(); // 155 counts=1V
         if (battery<620) BATGOOD=0;  // about 4V
         else BATGOOD=1;
         if ((!input(TRIGGER)) && SECTIMER>1) SWFLAG=1;  // If trigger switch pressed activate flag
         if (!input(SW2)) SW2FLAG=1;  // If SW2 activate flag
         checkmapsw(); // set map display if switch changes
      }

      if (EXTENDEDPUSH) //reset_cpu();
      {  addr=&STARTLAT;             // address of variable
         write_eepf(addr,101);         // save it
         addr=&STARTLONG;             // address of variable
         write_eepf(addr,105);         // save it
         addr=&LONGVAR;             // address of variable
         write_eepf(addr,109);         // save it
         write_eeprom(100,56);         // set a flag that this needs to be read
         delay_ms(200);
         reset_cpu();
      }

      // can put stuff here but make it quick

      goto stloop1;

   } // end of the nothing from GPS routine
   else // something has been received from the GPS
   {   // Could take about 422mS to get all chrs
      RS232_GPS(); // Receive all the RS232 characters Inbuilt check for trigger switch press
      get_gps_info(); // This sorts out the H,M,S Lat and Long
      if (GPSSTATUS==1)  // We have got valid data from GPS
      {  if (GOTREFXY==0)
         {  AWAITINGGPS=0; // Should now have data
            ALLOW=1; // Allow a sw press at anytime
            LCD_Setposition(LINE_1 + 0 );
            printf ( LCD_PutChar,"Need a Reference    ");
            LCD_Setposition(LINE_2 + 0 );
            printf ( LCD_PutChar,"Time and Position   ");
            LCD_Setposition(LINE_3 + 0 );
            printf ( LCD_PutChar,"Press the trigger   ");
            LCD_Setposition(LINE_4 + 0 );
            printf ( LCD_PutChar,"%f %f",LATITUDE,LONGITUDE);

         }
         else // already got a reference. Time and position
         {  findTXperiod(); //This sets the TXNUM. 0 to 7 (fOR tx 1 TO 8)
            get_currentxy(); // from the Latitude and Longitude work out our current xy
            PHASEDIS++;
            if (SW2FLAG)
            {  if (input(SW2)) // SW2 is off now
               {  DISPMODE=0; // goto text mode(0)
                  SW2FLAG=0; // Now reset
               }
            }
            if (BESTXYVALID[MAPTXNUM]==0) DISPMODE=0; // cannot do map
            if (DISPMODE==1) DISPLAY_MAP();
            else DISPLAY_TEXT();

            // Display line 1, the distance and bearing to the closest TX
            CLOSESTDISTANCE=32000;
            for(i=0;i<MAXTX;i++)
            {  if (BESTXYVALID[i]==1) // valid guess posn
               {  get_bearing_distance(CURRENTX,CURRENTY,BESTX[i],BESTY[i]); // get the Magnetic bearing from current point to best guess point
                  if (DISTANCE<CLOSESTDISTANCE)
                  {  CLOSESTDISTANCE=DISTANCE;
                     CLOSESTMAGBEARING=MAGBEARING;
                     CLOSESTTX=i+1;
                  }
               }
            }

            // Display the directions to the nearest TX
            LCD_Setposition(LINE_1 + 15 );
            printf ( LCD_PutChar,"    ");
            LCD_Setposition(LINE_1 + 7 );
            if (CLOSESTDISTANCE<10000)
            {  printf ( LCD_PutChar,"TX%u ",CLOSESTTX);
               printf ( LCD_PutChar,"%lim%li",CLOSESTDISTANCE,CLOSESTMAGBEARING);
               LCD_PutChar(0xDF); // degree symbol
               LCD_Setposition(LINE_1 + 15);
            }
            else printf ( LCD_PutChar,"           ");

            LCD_Setposition(LINE_1 + 19 );
            if (ALLOW==1) printf ( LCD_PutChar,"%u",(TXNUM+1));  // the current period
            else printf ( LCD_PutChar,"X");


            LCD_Setposition(LINE_4 + 16 );
            printf ( LCD_PutChar,"    ");
            LCD_Setposition(LINE_4 + 16 );
            printf ( LCD_PutChar,"%lu",lastbearing);
            LCD_PutChar(0xDF); // degree symbol

/*            if (BESTXYVALID[TXNUM]==1 && MAPSCREENINIT==0) // need to initialise info for map
            {  MAPSCREENINIT=1;
               g=abs(CURRENTX-BESTX[TXNUM]);
               g1=abs(CURRENTY-BESTY[TXNUM]);
               if (g>g1) STEP[TXNUM]=g/12;
               else STEP[TXNUM]=g1/12;
               STEP[TXNUM]++; // make it a minimum of 1, a bit bigger is better
             }
*/
         } // have a reference


         if (SECTIMER<200) SECTIMER++;  // increment to stop keep re-triggering the SWFLAG

         if (input(TRIGGER)) BUTTPRESSED=0; // if button is released
         else // button is  pressed
         {  BUTTPRESSED++;
            if (BUTTPRESSED>3) EXTENDEDPUSH=1;  // button is pressed for more than 3 secs
         }

         if (SWFLAG)  // trigger switch has been pressed, need to log this as a target bearing to a particular transmitter.
         {  SWFLAG=0;
            SECTIMER=0;
            bearingtofollow=lastbearing; // make this the bearing to follow
            startx=CURRENTX;
            starty=CURRENTY;
            // which TX period are we in
            if (ALLOW==1)
            { add_new_bearing();
            }
         }
      } // end of if gpsstatus=1


   } // end of the got something from GPS routine





   goto stloop1;

}