/*
 * Copyright (c) 2000 -- Wayne C. Gramlich
 * All rights reserved.
 *
 * Permission to use, copy, modify, distribute, and sell this software
 * for any purpose is hereby granted without fee provided that the above
 * copyright notice and this permission are retained.  The author makes
 * no representations about the suitability of this software for any purpose.
 * It is provided "as is" without express or implied warranty.
 *
 * This program computes the position P at (Px, Py) of a robot, given
 * fixed beacon locations A at (Ax, Ay), B at (Bx, By), and C at (Cx, Cy)
 * given aAPB and aBPC, the angle measured between A and B (at P) and the
 * angle measured between B and C (at P) respectively.
 *
 * Please note that the computed position becomes most sensitive to
 * small errors in measured angles when P is near the circle formed
 * by points A, B, and C.  When possible, you should position the
 * beacons so that they do not form a circle that will intersect
 * position that the robot may attempt to navigate from.
 *
 * This algorithm was published in "A Beacon Navigation Method for
 * Autonomous Vehicles" by Clare D. McGillem and Theodore S. Rappaport
 * published in IEEE Transactions on Vehicular Technology Vol. 38 No. 3
 * August 1989, pages 132-139.  The algorithm proper can be found in
 * equations (7) through (16).  The derivation of the algorithm can
 * be found in the appendix.
 *
 * The program uses the math library.  It can be compiled with:
 *
 *    cc -o locate -g locate.c -lm
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

int
main(
    int argc,
    char *argv[])
{
    double Ax, Ay;	/* Location of beacon A (given) */
    double Bx, By;	/* Location of beacon B (given) */
    double Cx, Cy;	/* Location of beacon C (given) */
    double aAPB;	/* Angle between A and B at P in degrees (given) */
    double aBPC;	/* Angle between B and C at P in degrees (given) */

    /*
     * The algorithm computes the point N=(Nx, Ny) which is the center
     * of a circle of radius rABP that goes through points A, B, and P.
     * Similarly, the algorithm computes the point M=(Mx, My) which is
     * the center of the a circle of radius rBCP that goes through
     * points B, C, and P.
     */
    double pi;		/* 3.14159 (given) */
    double dAB;		/* Distance between A and B (computed) */
    double dBC;		/* Distance between B and C (computed) */
    double dABx, dABy;	/* Temporary results */
    double dBCx, dBCy;	/* Temporary results */
    double rABP, rBCP;	/* Radius of circles ABP and BCP (computed) */
    double Mx, My;	/* Center of circle BCP (computed) */
    double Nx, Ny;	/* Center of circle ABP (computed) */
    double Px, Py;	/* Position of robot (computed) */
    double aXAB;	/* Beacon B bearing from virtual X axis through A */
    double aXBC;	/* Beacon C bearing from virtual X axis through B */
    double m, n;	/* Magic intermediate results (computed) */

    /* Grab the input arguments from the command line: */
    argc--;
    argv++;
    if (argc < 8) {
	(void)printf("Usage: compute Ax Ay Bx By Cx Cy aAPB aBPC\n");
	(void)printf("aAPB and aBPC are in degrees\n");
	return 0;
    }
    Ax = atof(argv[0]);
    Ay = atof(argv[1]);
    Bx = atof(argv[2]);
    By = atof(argv[3]);
    Cx = atof(argv[4]);
    Cy = atof(argv[5]);
    aAPB = atof(argv[6]);
    aBPC = atof(argv[7]);
    (void)printf("A=%.3f:%.3f B=%.3f:%.3f C=%.3f:%.3f ABP=%.3f aBPC=%.3f\n",
      Ax, Ay, Bx, By, Cx, Cy, aAPB, aBPC);

    /* Convert angles ABP and aBPC to radians: */
    pi = 3.14159;
    aAPB *= pi/180.;
    aBPC *= pi/180.;

    /* Compute distances between AB and BC: */
    dABx = Ax - Bx;
    dABy = Ay - By;
    dAB = sqrt(dABx*dABx + dABy*dABy);
    dBCx = Bx - Cx;
    dBCy = By - Cy;
    dBC = sqrt(dBCx*dBCx + dBCy*dBCy);
    (void)printf("dAB=%.3f dBC=%.3f\n", dAB, dBC);

    /* Compute rABP and rBCP, the radius of each circle: */
    rABP = dAB / (2. * sin(aAPB));
    rBCP = dBC / (2. * sin(aBPC));
    (void)printf("rABP=%.3f rBCP=%.3f\n", rABP, rBCP);

    /* Compute bearings between beacons A, B, and C: */
    aXAB = atan((By - Ay) / (Bx - Ax));
    aXBC = atan((Cy - By) / (Cx - Bx));
    (void)printf("aXAB=%.3f aXBC=%.3f\n", aXAB*180./pi, aXBC*180./pi);

    /* N and M are the centers of the circles for ABP and BCP respectively: */
    Nx = Ax - rABP * sin(aXAB - aAPB);
    Ny = Ay - rABP * cos(aXAB - aAPB);
    Mx = Bx - rBCP * sin(aXBC - aBPC);
    My = By - rBCP * cos(aXBC - aBPC);
    (void)printf("M=(%.3f:%.3f) N=(%.3f:%.3f)\n", Nx, Ny, Mx, My);

    /* Compute n and m, two magic intermediate results: */
    m = (My - Ny) / (Nx - Mx);
    n = (rBCP*rBCP - rABP*rABP - Mx*Mx + Nx*Nx - My*My + Ny*Ny) /
      (2. * (Nx - Mx));
    (void)printf("m=%.5f n=%.5f\n", m, n);

    /* Compute position Px and Py: */
    Py = 2.0 * (m * Nx + Ny + m * n) / (1.0 + m * m) - By;
    Px = m * Py + n;
    (void)printf("P=(%.3f,%.3f)\n", Px, Py);
    return 0;
}