!     Last change:  A     9 Jul 2005    9:59 pm
PROGRAM Triangulation !For MPDSS: Multi Ping Distance Sensor Systems
! by: Alex Beck.
   IMPLICIT NONE
   REAL(kind=8),PARAMETER::pi = 3.1415926535897932384626433832795D0, small = 0.1, tolerance = 0.0001
   INTEGER::A,F,G,H,I,J,K,L
REAL(kind=8)::distance0,distance1,distance2,distance3,S0X,S0Y,S0Z,S1X,S1Y,S1Z,S2X,S2Y,S2Z,S3X,S3Y,S3Z,TX,TY,TZ,B,C,E
   REAL(kind=8), DIMENSION(0:5)::D
   REAL(kind=8), DIMENSION(0:5)::X,Y,Z
   !distance0-3 is a sinuglar element of distance mesured from each sensor-node subsystem. D0-3 are temp variables for each sensor.
   !sensor-node subsystem data. S0-3X-Z represents the sensors (X,Y,Z) cartesian coordinate data received at WDT. X,Y,Z are temp.

   !WRITE (*,*) "distance0: "
   !READ  (*,*) distance0
   !WRITE (*,*) "distance1: "
   !READ  (*,*) distance1
   !WRITE (*,*) "distance2: "
   !READ  (*,*) distance2
   !WRITE (*,*) "distance3: "
   !READ  (*,*) distance3

   distance0 = 1466785.0D0
   distance1 = 489634.0D0
   distance2 = 752222.0D0
   distance3 = 1010101.0D0

   S0X = 0.0D0
   S0Y = 0.0D0
   S0Z = 0.0D0

   S1X = 1000000.0D0
   S1Y = 0.0D0
   S1Z = 0.0D0

   S2X =  500000.0D0
   S2Y = 1000000.0D0
   S2Z = 1000000.0D0

   S3X = 0.0D0
   S3Y = 1200000.0D0
   S3Z = 0.0D0

   X(0) = S0X + distance0
   Y(0) = S0Y
   Z(0) = S0Z
   CALL distanceFinder(X(0),Y(0),Z(0), S1Y,S1X,S1Z,D(0))

   X(1) = S0X
   Y(1) = S0Y + distance0
   Z(1)= S0Z
   CALL distanceFinder(X(1),Y(1),Z(1), S1Y,S1X,S1Z,D(1))

   X(2) = S0X - distance0
   Y(2) = S0Y
   Z(2) = S0Z
   CALL distanceFinder(X(2),Y(2),Z(2), S1Y,S1X,S1Z,D(2))

   X(3) = S0X
   Y(3) = S0Y - distance0
   Z(3) = S0Z
   CALL distanceFinder(X(3),Y(3),Z(3), S1Y,S1X,S1Z,D(3))

   IF (ABS(D(0)-distance1) < ABS(D(1)-distance1)) THEN
      A = 0
     ELSE
      A = 1
   END IF

   IF (ABS(D(2)-distance1) < ABS(D(A)-distance1)) THEN
      A = 2
   END IF

   IF (ABS(D(3)-distance1) < ABS(D(A)-distance1)) THEN
      A = 3
   END IF

   WRITE (*,*) "A: ",A

   X(0)=X(A)
   Y(0)=Y(A)
   Z(0)=Z(A)

   Z(1) = Z(0) + small
   Z(2) = Z(0) - small

   IF (A==0 .OR. A==2) THEN
      Y(1)=Y(0)
      Y(2)=Y(0)
      CALL Sphere(distance0, Y(1), Z(1), X(1))
      X(2) = X(1)
   ELSE
      X(1)=X(0)
      X(2)=X(0)
      CALL Sphere(distance0, X(1), Z(1), Y(1))
      Y(2) = Y(1)
   END IF

   CALL distanceFinder(X(0),Y(0),Z(0),S1Y,S1X,S1Z,D(0))
   CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
   CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

   IF ( ABS( D(2)-distance1 ) < ABS( D(1)-distance1 ) ) THEN !Checks which side of the equator is closer
      X(3) = X(2)
      Y(3) = Y(2) !OVERWRITES chosen variables into '1' array slot, or leaves them there
      Z(3) = Z(2)
      D(3) = D(2)

   ELSE
      X(3) = X(1)
      Y(3) = Y(1) !OVERWRITES chosen variables into '1' array slot, or leaves them there
      Z(3) = Z(1)
      D(3) = D(1)
   END IF

   IF (ABS(D(0)-distance1) < ABS(D(3)-distance1)) THEN !then equator point closer
      WRITE (*,*) "Take care of this occurance with below GOTO statement."
      !!GOTO
   END IF

   IF (A==0 .OR. A==2) THEN !inputs distances from other equator points near chosen point defined by A
         Y(1) = Y(0) + small !!! It could be that Y(0) in this instance is always 0
         Z(1) = Z(0)
         CALL Sphere(distance0, Y(1), Z(1), X(1))
         Y(2) = Y(0) - small !!! It could be that Y(0) in this instance is always 0
         Z(2) = Z(0)
         X(2) = X(1)         !!! It could be that this statment may need to be corrected.

         CALL distanceFinder(X(0),Y(0),Z(0),S1Y,S1X,S1Z,D(0))
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )
         C = ABS( D(3) - distance1)
         WRITE (*,*) C
         READ (*,*) E
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF

         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         Y(3) = (Y(1) - Y(2))/2.0D0 + Y(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

      END DO
      X(1) = X(3)
      Y(1) = Y(3)
      Z(1) = Z(3)


      X(4) = X(1) - X(0)                    !Begin finding location of point along 90 degree arc on sphere
      Y(4) = Y(1) - Y(0)
      Z(4) = Z(1) - Z(0)

      B = -X(0)/X(4)
      X(2) = 0
      Y(2) = Y(1)*B + Y(0)
      Z(2) = Z(1)*B + Z(0)
      CALL Sphere(distance0, Y(2), X(2), Z(3))
      IF (ABS(Z(3) - Z(2)) < tolerance) THEN
         WRITE (*,*) "WORks"
      ELSE
         WRITE (*,*) "MIGHT BE A PROBLEM"
      END IF


   ELSE

         X(1) = X(0) + small !!! It could be that Y(0) in this instance is always 0
         Z(1) = Z(0)
         CALL Sphere(distance0, X(1), Z(1), Y(1))
         X(2) = X(0) - small !!! It could be that Y(0) in this instance is always 0
         Z(2) = Z(0)
         Y(2) = Y(1)         !!! It could be that this statment may need to be corrected.

         CALL distanceFinder(X(0),Y(0),Z(0),S1Y,S1X,S1Z,D(0))
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )      !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp
         C = ABS( D(3) - distance1)                         !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp
         WRITE (*,*) C                                      !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp
         READ (*,*) E                                       !!!!!!!!!!!!!!!!!!!!!!!!!ttttttttttttttrrrrrrrrrrrraaaaaaaaappppppppp
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF

         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         X(3) = (X(1) - X(2))/2.0D0 + X(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

      END DO
      X(1) = X(3)
      Y(1) = Y(3)
      Z(1) = Z(3)


      X(4) = X(1) - X(0)                    !Begin finding location of point along 90 degree arc on sphere
      Y(4) = Y(1) - Y(0)
      Z(4) = Z(1) - Z(0)

      B = -Y(0)/Y(4)
      Y(2) = 0
      X(2) = X(1)*B + X(0)
      Z(2) = Z(1)*B + Z(0)
      CALL Sphere(distance0, Y(2), X(2), Z(3))
      IF (ABS(Z(3) - Z(2)) < tolerance) THEN
         WRITE (*,*) "WORks"
      ELSE
         WRITE (*,*) "MIGHT BE A PROBLEM"
      END IF                                                                     !!!!!!!!!!!!!!!!!!!!!!!!!!!11    !!!!!!!!!!!!!!!!!
                                                                            !!!!!!!!!!!!!!!!!!!LOOOK FOR ERROR IN BINARY SELECTOR
   END IF                      !!!!!!!!!!!!!!!                                   ! NOT DENING NEW 1 POSITION PARAMETER
                                                                                                !!!!!!!!!!!!!!!
                        !!!!!!!!!!!!!!!!!!!!!!!!!!!! SHOOTING POINT AFTER AIMING WITH ABOVE DO LOOP!!!!!!!!!!!!!!!!!!!!1111
                                          !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE
   IF (A==0 .OR. A==2) THEN !defines second point for finding cirlcle of overlap of distance0 and distance1 locks down bisection.

      X(2) = 0.0D0
      C = Z(1)/Y(1)
      IF (Y(1) - Y(0) > 0.0D0) THEN
         Y(2) = + distance0/(SQRT(C*C+1.0D0))
      ELSE
         Y(2) = - distance0/(SQRT(C*C+1.0D0))
         CALL Sphere(distance0, X(2), Y(2), Z(2))
         !!!!
         Z(4) = C*Y(2)
         IF (ABS(Z(4) - Z(2)) < small) THEN
            WRITE (*,*) "MAKE PROGRAM FASTER"
         ELSE
            WRITE (*,*) "WE COULD HAVE A PROBLEM"
         END IF
         !!!!
      END IF

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
         Z(3) = C*Y(3)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO                                                                       !SHOULD be locked down on cirlce at this poiint

      ELSE

      Y(2) = 0.0D0
      C = Z(1)/X(1)
      IF (X(1) - X(0) > 0.0D0) THEN
         X(2) = + distance0/(SQRT(C*C+1.0D0))
      ELSE
         X(2) = - distance0/(SQRT(C*C+1.0D0))
         CALL Sphere(distance0, Y(2), X(2), Z(2))
         !!!!
         Z(4) = C*X(2)
         IF (ABS(Z(4) - Z(2)) < small) THEN
            WRITE (*,*) "MAKE PROGRAM FASTER"
         ELSE
            WRITE (*,*) "WE COULD HAVE A PROBLEM"
         END IF
         !!!!
      END IF

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         X(3) = (X(2)-X(1))/2.0D0 + X(1)
         Z(3) = C*X(3)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF

   X(0) = X(3)                                                 !X,Y,Z(0) NOW REPRESENTS POINT ON CIRCLE WHERE d0 and d1 intersect
   Y(0) = Y(3)
   Z(0) = Z(3)

   X(1) = X(0) + small*10.0D0                                               !Defines 4 points around cirlce point
   Y(1) = Y(0)
   CALL Sphere(distance0, Y(1), X(1), Z(1))
   CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))

   X(2) = X(0) - small*10.0D0
   Y(2) = Y(0)
   CALL Sphere(distance0, Y(2), X(2), Z(2))
   CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

   Y(3) = Y(0) + small*10.0D0
   X(3) = X(0)
   CALL Sphere(distance0, Y(3), X(3), Z(3))
   CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

   Y(4) = Y(0) - small*10.0D0
   X(4) = X(0)
   CALL Sphere(distance0, Y(4), X(4), Z(4))
   CALL distanceFinder(X(4),Y(4),Z(4),S1Y,S1X,S1Z,D(4))

   A = 1

   IF ( ABS(D(2) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(2)
      Y(1) = Y(2)
      Z(1) = Z(2)
      A = 2
   END IF

   IF ( ABS(D(3) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(3)
      Y(1) = Y(3)
      Z(1) = Z(3)
      A = 3
   END IF

   IF ( ABS(D(4) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(4)
      Y(1) = Y(4)
      Z(1) = Z(4)
      A = 4
   END IF

   IF (A==3 .OR. A==4) THEN

      X(2) = X(1) + small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      Y(2) = Y(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      X(3) = X(1) - small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      Y(3) = Y(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)
      END IF

      X(3) = (X(2)-X(1))/2.0D0 + X(1)
      Y(3) = Y(2)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )                      !Bisection method to lock down on another circle point
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         X(3) = (X(2)-X(1))/2.0D0 + X(1)
         Y(3) = Y(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF                                              !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE

   IF (A==1 .OR. A==2) THEN

      Y(2) = Y(1) + small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      X(2) = X(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      Y(3) = Y(1) - small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      X(3) = X(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)
      END IF

      Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
      X(3) = X(2)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )                      !Bisection method to lock down on another circle point
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
         X(3) = X(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF

   X(4) = X(3) !temporarily stores second point in X,Y,Z(4)
   Y(4) = Y(3)
   Z(4) = Z(3)

                !!!!!!!!!!!!!!!!DOES ENTIRE ABOVE OPERATION OVER AGAIN TO DEDUCE THIRD POINT FROM SECOND POINT

   X(1) = X(4) + small*10.0D0                                               !Defines 4 points around cirlce point
   Y(1) = Y(4)
   CALL Sphere(distance0, Y(1), X(1), Z(1))
   CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))

   X(2) = X(4) - small*10.0D0
   Y(2) = Y(4)
   CALL Sphere(distance0, Y(2), X(2), Z(2))
   CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

   Y(3) = Y(4) + small*10.0D0
   X(3) = X(4)
   CALL Sphere(distance0, Y(3), X(3), Z(3))
   CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

   Y(4) = Y(4) - small*10.0D0
   X(4) = X(4)
   CALL Sphere(distance0, Y(4), X(4), Z(4))
   CALL distanceFinder(X(4),Y(4),Z(4),S1Y,S1X,S1Z,D(4))

   A = 1

   IF ( ABS(D(2) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(2)
      Y(1) = Y(2)
      Z(1) = Z(2)
      A = 2
   END IF

   IF ( ABS(D(3) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(3)
      Y(1) = Y(3)
      Z(1) = Z(3)
      A = 3
   END IF

   IF ( ABS(D(4) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(4)
      Y(1) = Y(4)
      Z(1) = Z(4)
      A = 4
   END IF

   IF (A==3 .OR. A==4) THEN

      X(2) = X(1) + small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      Y(2) = Y(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      X(3) = X(1) - small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      Y(3) = Y(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)
      END IF

      X(3) = (X(2)-X(1))/2.0D0 + X(1)
      Y(3) = Y(2)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )                      !Bisection method to lock down on another circle point
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         X(3) = (X(2)-X(1))/2.0D0 + X(1)
         Y(3) = Y(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF                                              !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE

   IF (A==1 .OR. A==2) THEN

      Y(2) = Y(1) + small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      X(2) = X(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      Y(3) = Y(1) - small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      X(3) = X(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)
      END IF

      Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
      X(3) = X(2)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )                      !Bisection method to lock down on another circle point
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
         X(3) = X(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF

   X(5) = X(3)  !STORES third CIRCLE POINT IN X,Y,Z(5)
   Y(5) = Z(3)
   Y(5) = Z(3)

                 !!!!!!!!!!!!!!!!DOES ENTIRE ABOVE OPERATION OVER AGAIN TO DEDUCE THIRD POINT FROM SECOND POINT

   X(1) = X(5) + small*10.0D0                                               !Defines 4 points around cirlce point
   Y(1) = Y(5)
   CALL Sphere(distance0, Y(1), X(1), Z(1))
   CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))

   X(2) = X(5) - small*10.0D0
   Y(2) = Y(5)
   CALL Sphere(distance0, Y(2), X(2), Z(2))
   CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

   Y(3) = Y(5) + small*10.0D0
   X(3) = X(5)
   CALL Sphere(distance0, Y(3), X(3), Z(3))
   CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

   Y(4) = Y(5) - small*10.0D0
   X(4) = X(5)
   CALL Sphere(distance0, Y(4), X(4), Z(4))
   CALL distanceFinder(X(4),Y(4),Z(4),S1Y,S1X,S1Z,D(4))

   A = 1

   IF ( ABS(D(2) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(2)
      Y(1) = Y(2)
      Z(1) = Z(2)
      A = 2
   END IF

   IF ( ABS(D(3) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(3)
      Y(1) = Y(3)
      Z(1) = Z(3)
      A = 3
   END IF

   IF ( ABS(D(4) - distance1) < ABS(D(1) - distance1) ) THEN
      X(1) = X(4)
      Y(1) = Y(4)
      Z(1) = Z(4)
      A = 4
   END IF

   IF (A==3 .OR. A==4) THEN

      X(2) = X(1) + small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      Y(2) = Y(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      X(3) = X(1) - small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      Y(3) = Y(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)
      END IF

      X(3) = (X(2)-X(1))/2.0D0 + X(1)
      Y(3) = Y(2)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )                      !Bisection method to lock down on another circle point
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         X(3) = (X(2)-X(1))/2.0D0 + X(1)
         Y(3) = Y(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF                                              !!!!!!!! SAVE OPERATIONAL SPEED BY NESTING BELOW IF UNDER ELSE OF ABOVE

   IF (A==1 .OR. A==2) THEN

      Y(2) = Y(1) + small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      X(2) = X(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))

      Y(3) = Y(1) - small*10.0D0     !Defines a new point X,Y,Z(2) which is perpendicular to X,Y,Z(1)
      X(3) = X(1)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      IF ( ABS( D(3) - distance1 ) < ABS( D(2) - distance1 ) ) THEN
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)
      END IF

      Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
      X(3) = X(2)
      CALL Sphere(distance0, Y(3), X(3), Z(3))
      CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))

      DO WHILE ( ABS( D(3) - distance1 ) > tolerance )                      !Bisection method to lock down on another circle point
         CALL distanceFinder(X(1),Y(1),Z(1),S1Y,S1X,S1Z,D(1))
         CALL distanceFinder(X(2),Y(2),Z(2),S1Y,S1X,S1Z,D(2))
         IF ( ABS( D(2) - distance1 ) < ABS( D(1) - distance1 ) ) THEN
            X(1) = X(2)
            Y(1) = Y(2)
            Z(1) = Z(2)
         END IF
         X(2) = X(3)
         Y(2) = Y(3)
         Z(2) = Z(3)

         Y(3) = (Y(2)-Y(1))/2.0D0 + Y(1)
         X(3) = X(2)
         CALL Sphere(distance0, Y(3), X(3), Z(3))

         CALL distanceFinder(X(3),Y(3),Z(3),S1Y,S1X,S1Z,D(3))
      END DO
   END IF

                  ! X,Y,Z(0) X,Y,Z(1) X,Y,Z(2) X,Y,Z(3)  STORES ALL THREE POINTS ON CIRLCE

   X(1) = X(4)
   Y(1) = Y(4)
   Z(1) = Z(4)

   X(2) = X(4)
   Y(2) = Y(4)
   Z(2) = Z(4)






STOP

CONTAINS

SUBROUTINE distanceFinder (X, Y, Z, SX, SY, SZ, distance) !Determines distance from X,Y,Z coordinates to Sensor's location
   REAL(KIND=8), INTENT(IN)  :: X, Y, Z, SX, SY, SZ
   REAL(KIND=8), INTENT(OUT) :: distance
   distance = SQRT((SX-X)*(SX-X) + (SY-Y)*(SY-Y) + (SZ-Z)*(SZ-Z))
 RETURN
END SUBROUTINE distanceFinder

SUBROUTINE Sphere (R, X, Y, Z) !extrapolates the third cooridinate (always positive) given 2 coordinates and the radius.
   REAL(KIND=8), INTENT(IN)  :: R, X, Y
   REAL(KIND=8), INTENT(OUT) :: Z
   Z = SQRT( -(X*X) - (Y*Y) + (R*R) )
 RETURN
END SUBROUTINE Sphere

SUBROUTINE Circle (R, X, Y) !extrapolates the third cooridinate (always positive) given 2 coordinates and the radius.
   REAL(KIND=8), INTENT(IN)  :: R, X
   REAL(KIND=8), INTENT(OUT) :: Y
   Y = SQRT( -(X*X) + (R*R) )
 RETURN
END SUBROUTINE Circle

END PROGRAM Triangulation
