PRINT "RINGRIG.BAS 15Feb15 tests direction calculation from AUTOP9TTS in circle 0 to 359"
OPEN "RINGRIG.TXT" FOR OUTPUT AS #1
PRINT "Angle   W4(LatB)  W5(LonB)      W1(Calculated)   Error"
rmserror = 0 'RMS (Root Mean Square) value of error from all 360 tests.

'need to declare as integers, or will default to real floating values ?
'DIM W1, W2, W3, W4, W5, w6, w7, w8, w9, w10, w11, w12, w13, b0, b1, b14, b15, b22, b23 AS LONG 'Picaxe variables
DIM W1 AS LONG: DIM W2 AS LONG: DIM W3 AS LONG: DIM W4 AS LONG: DIM W5 AS LONG: DIM w6 AS LONG
DIM W7 AS LONG: DIM w8 AS LONG: DIM w9 AS LONG: DIM W10 AS LONG: DIM w11 AS LONG: DIM w12 AS LONG
DIM w13 AS LONG: DIM b0 AS LONG: DIM b1 AS LONG: DIM b14 AS LONG: DIM b15 AS LONG: DIM b22 AS LONG
DIM b23 AS LONG: DIM i AS LONG: DIM ii AS LONG



'keep values in rig lower for if 16 bit integers were used

DIM A, LatD, LonD, COSLATREAL AS SINGLE  'real ringrig variables
LatD = 10000: LonD = 10000: R = 5000 'real ringrig variables
COSLATREAL = COS(51 * 3.14159 / 180)

FOR i = 0 TO 359 STEP 1  '0 to 359 degrees from destination out to boat
  ii = i + 180 'ii is direction from boat back to destination = that calculated
  IF ii > 359 THEN
    ii = ii - 360
  END IF
  A = i * 3.14159 / 180 'A in radians
  LatB = LatD + COS(A) * R 'Latitude of Boat
  LonB = LonD - SIN(A) * R / COSLATREAL 'Longitude of Boat (Note degrees WEST are less, and /COS)

  'Feed input into autopilot logic in w4, w5, w8, w9 to get calculated angle in w1.
  b0 = 51: W4 = INT(LatB):  b1 = 0: W5 = INT(LonB) 'Rig calculated Boat GPS Position.
  b14 = 51: w8 = LatD: b15 = 0: w9 = LonD 'Rig fixed destination position.



'Imported code from AUTOP9TTS.BAS 13Feb15

 'calculate approximate target heading based on difference in lat/lons.
 'This based on just minutes if two positions have same lat and lon degrees.
 'If different degrees, then scale minutes and modify based on degrees.
 'Scaling is used to avoid overflow when a long distance appart.
 'This arithmetic overwrites the values of GPS and Destination lat/lons.
 'Temp variables available: w6,w11,w12.

 '1v change minutes if not same values of degrees for GPS and Destination.
 IF b0 <> b14 OR b1 <> b15 THEN 'NOT same value of degrees in lat or lon
  'work with units of 0.1 minutes of lat/lon i.e. 100x less accurate.
  w11 = b0 * 600: w12 = W4 / 100 '1t was *300 and /50 GPS lat/lon
  W4 = w11 + w12 'latitude LSD is 0.1 minutes = 1/600th degree
  w11 = b1 * 600: w12 = W5 / 100
  W5 = w11 + w12 'longitude LSD is 0.1 minutes = 1/600th degree
  w11 = b14 * 600: w12 = w8 / 100 'Destination lat/lon
  w8 = w11 + w12 'latitude LSD is 0.1 minutes = 1/600th degree '26Jan15 Problem?
  w11 = b15 * 600: w12 = w9 / 100
  w9 = w11 + w12 'longitude LSD is 0.1 minutes = 1/600th degree '26Jan15 Problem?
 END IF
 
 '09Feb15 use new COSLAT function near end of program, instead of earlier approximation.
 '13Feb15 call coslat 'get 100 * Cos(Lat) where Lat is average of GPS and Destination latitudes.
 '13Feb15 b1 = b22 / 10 'original code using b1 can now be used....
 'now just the values of minutes (or scaled minutes) can be used ...
   
 IF w8 > W4 THEN 'Dest Lat > GPS Lat
   w12 = w8 - W4 'Diff Lat
   IF w9 < W5 THEN 'Dest Lon east of GPS Lon
     w13 = W5 - w9 'Diff lon
     GOSUB scalew12w13 'scale if either more than 1000 to avoid overflow
   ELSE 'Dest Lon west of GPS Lon
     w13 = w9 - W5 'Diff lon
     GOSUB scalew12w13 'scale if either more than 1000 to avoid overflow
     w6 = 359 ''12Feb15 was 360
     w13 = w6 - w13 'was w13 = 360 - w13
   END IF
 ELSE 'Dest Lat < GPS Lat
   w12 = W4 - w8 'Diff Lat
   IF w9 < W5 THEN 'Dest Lon east of GPS Lon
     w13 = W5 - w9 'Diff lon
     GOSUB scalew12w13 'scale if either more than 1000 to avoid overflow
     w6 = 180
     w13 = w6 - w13 'was w13 = 180 - w13
   ELSE 'Dest Lon west of GPS Lon
     w13 = w9 - W5 'Diff lon
     GOSUB scalew12w13 'scale if either more than 1000 to avoid overflow
     '12Feb15  w6 = 180
     w13 = w13 + 180 'was w13 = w6 + w13 'was w13 + 180
   END IF
 END IF
 
 W1 = w13 'make this the target heading THIS MAKES THE METHOD ACTIVE
 GOTO afterscalesub '1t was x so subroutine near place called from - for clarity

scalew12w13: '13Feb15 scale w12 and w13 to less than 600 so can * by up to 100
 IF w12 < 600 AND w13 < 600 THEN
   GOTO endscale
 END IF
 w12 = w12 / 2: w13 = w13 / 2 'make both smaller
 GOTO scalew12w13 'repeat until both less than 600
endscale:
'13Feb15 if w12 > 1000 or w13 > 1000 then
'13Feb15   w12 = w12 / 100
'13Feb15   w13 = w13 / 100
'13Feb15 endif
 GOSUB coslat 'get 100 * Cos(Lat) where Lat is average of GPS and Destination latitudes.
 w13 = w13 * b22 / 100 '13Feb15 old code was w13 = w13 * b1 / 10 '1v b1 was COSLAT
 IF w13 < w12 THEN
   w13 = w13 * 45 / w12
 ELSE
   w12 = w12 * 45 / w13
   w13 = 90 - w12
 END IF
 RETURN


coslat:   '09Feb15 returns COS(Lat) * 100 in b22. e.g. COS(50)=0.64. b22=64.
   '13Feb15 COS=114-Lat e.g. 64 for Lat 51. Was 115 so less accurate at 51.
   'Average Lat between GPS in b0 and Destination waypoint in b14 is used.
   b22 = b0 + b14: b22 = b22 / 2 'average.
   b23 = 114: b22 = b23 - b22 'approximation in b22 is 115 - Lat in degrees. 13Feb15
   RETURN

 
afterscalesub:
aftercalchdg:  'W1 now holds the Target Heading that the boat must steer towards

'END OF IMPORTED CODE FROM AUTOP9TTS.BAS

  errorv = W1 - ii 'error in degrees between direction and calculated direction
  if errorv >= 180 then 'true error can only be 0 to 180
    errorv = 360 - errorv
  endif
  rmserror = rmserror + ( errorv * errorv )


  PRINT "II="; ii; " W4(LatB)="; W4; " W5(LonB)="; W5; " W1(A)="; W1; "Error="; errorv
  PRINT #1, "II="; ii; " W4(LatB)="; W4; " W5(LonB)="; W5; " W1(A)="; W1; "Error="; errorv

NEXT

rmserror = SQR(rmserror/360)
PRINT "RMS (Root Mean Square) value of all errors = "; rmserror
PRINT #1, "RMS (Root Mean Square) value of all errors = "; rmserror

CLOSE #1

STOP