/*
 * $Id: compute.c,v 2.7 90/06/15 21:31:55 tynor Exp $
 *----------------------------------------------------------------------------
 *	FPLAN - Flight Planner
 *	Steve Tynor
 *	tynor@prism.gatech.edu
 *
 *	This program is in the public domain. Permission to copy,
 * distribute, modify this program is hearby given as long as this header
 * remains. If you redistribute this program after modifying it, please
 * document your changes so that I do not take the blame (or credit) for
 * those changes.  If you fix bugs or add features, please send me a
 * patch so that I can keep the 'official' version up-to-date.
 *
 *	Bug reports are welcome and I'll make an attempt to fix those
 * that are reported.
 *
 *	USE AT YOUR OWN RISK! I assume no responsibility for any
 * errors in this program, its database or documentation. I will make an
 * effort to fix bugs, but if you crash and burn because, for example,
 * fuel estimates in this program were inaccurate, it's your own fault
 * for trusting somebody else's code! Remember, as PIC, it's _your_
 * responsibility to do complete preflight planning. Use this program as
 * a flight planning aid, but verify its results before using them.
 *----------------------------------------------------------------------------
 */

static char rcsid[] = "$Id: compute.c,v 2.7 90/06/15 21:31:55 tynor Exp $";

#include "wp_info.h"
#include <math.h>

/*----------------------------------------------------------------------------*/
static void wind_correct (true_air_speed, mag_course, 
			  wind_speed, wind_heading,
			  ground_speed, mag_heading)
     double true_air_speed, mag_course, 
	wind_speed, wind_heading, 
	*ground_speed, *mag_heading;
{
   double hdiff, tx, ty, wca;

   if (wind_heading <= 180.0)
      wind_heading += 180.0;
   else
      wind_heading -= 180.0;
   hdiff = DEG2RAD (mag_course - wind_heading);
   tx = wind_speed * sin (hdiff);
   ty = wind_speed * cos (hdiff);
   wca = asin (tx / true_air_speed);
   *ground_speed = cos (wca) * true_air_speed + ty;
   *mag_heading = mag_course + RAD2DEG (wca);
   if (*mag_heading >= 360.0)
      *mag_heading -= 360.0;
   else if (*mag_heading <= 0.0)
      *mag_heading += 360.0;
}

/*----------------------------------------------------------------------------*/
void distance_and_heading (lat1, long1, lat2, long2, distance, heading)
     double lat1, long1, lat2, long2;
     double *distance, *heading;
{
   double tmp;

   lat1  = DEG2RAD (lat1);
   long1 = DEG2RAD (long1);
   lat2  = DEG2RAD (lat2);
   long2 = DEG2RAD (long2);

   if ((lat1 == lat2) && (long1 == long2)) {
      *distance = 0.0;
      *heading  = 0.0;
      return;
   }

   tmp = atan2 (2.0 * asin (sin ((long1 - long2) / 2.0)),
		log (tan (PI / 4.0 + lat2 / 2.0)) - 
		log (tan (PI / 4.0 + lat1 / 2.0)) );
   if (lat1 == lat2)
      *distance = 60.0 *
	 fabs (RAD2DEG (2.0 * asin (sin ((long1 - long2) / 2.0)))) *
	    cos (lat1);
   else
      *distance = 60.0 * RAD2DEG (lat2 - lat1) / cos (fabs (tmp));
   
   if (asin (sin (long1-long2)) >= 0.0)
      *heading = RAD2DEG (fabs(tmp));
   else
      *heading = 360.0 - RAD2DEG (fabs(tmp));
}

/*----------------------------------------------------------------------------*/
static int next_non_incremental (start)
     int start;
{
   int i = start;

   for (i = start; i < num_waypoints; i++)
      if (waypoints[i].db->mode != WP_INCREMENTAL)
	 return i;
   return -1;
}

/*----------------------------------------------------------------------------*/
static void locate_incrementals ()
{
   int    j, prev, next;
   double d, heading;

   for (prev = 0; ((prev >= 0) && (prev < num_waypoints)); ) {
      next = next_non_incremental (prev+1);
      if (next >= 0) {
	 distance_and_heading (waypoints[prev].db->latitude,
			       waypoints[prev].db->longitude, 
			       waypoints[next].db->latitude, 
			       waypoints[next].db->longitude, 
			       &d, &heading);
	 for (j = prev+1; j < next; j++) {
	    if (waypoints[j].db->u.dist_since_last_wp >= 0.0) {
	       waypoints[j].db->latitude =  waypoints[prev].db->latitude +
		  (waypoints[next].db->latitude - waypoints[prev].db->latitude)
		     * waypoints[j].db->u.dist_since_last_wp / d;
	       waypoints[j].db->longitude =  waypoints[prev].db->longitude +
		  (waypoints[next].db->longitude -
		   waypoints[prev].db->longitude) * 
		      waypoints[j].db->u.dist_since_last_wp / d;
	    } else {
	       waypoints[j].db->latitude =  waypoints[next].db->latitude +
		  (waypoints[next].db->latitude - waypoints[prev].db->latitude)
		     * waypoints[j].db->u.dist_since_last_wp / d;
	       waypoints[j].db->longitude =  waypoints[next].db->longitude +
		  (waypoints[next].db->longitude -
		   waypoints[prev].db->longitude) * 
		      waypoints[j].db->u.dist_since_last_wp / d;
	    }
 	    waypoints[j].db->mag_variation =
	       waypoints[prev].db->mag_variation + 
		  (waypoints[next].db->mag_variation -
		   waypoints[prev].db->mag_variation) *
 		   waypoints[j].db->u.dist_since_last_wp / d;
	 }
      }
      prev = next;
   }
}

/*----------------------------------------------------------------------------*/
#define IS_NAVAID(mode) (((mode) == WP_VOR) || ((mode) == WP_NDB) || \
			 ((mode) == WP_DME) || ((mode) == WP_TAC) || \
			 ((mode) == WP_ILS) || ((mode) == WP_LOM) || \
			 ((mode) == WP_LMM))

/*----------------------------------------------------------------------------
 * FUZZ is the number of degrees two points can be apart and still be considered
 * colocated - we let this be fuzzy, since it's often the case that onfield VORs
 * are reported with slightly different lat/long, but we still want to view them
 * as the same point...
 */
#define FUZZ 0.01
#define COLOCATED(db1,db2) \
   ((fabs ((db1)->latitude - (db2)->latitude) <= FUZZ) && \
    (fabs ((db1)->longitude - (db2)->longitude) <= FUZZ))

/*----------------------------------------------------------------------------*/
static DATABASE_INFO *get_navaid (wp)
     WAYPOINT_INFO wp;
     /*
      * If the waypoint is a navaid return its db pointer. If it's an airport 
      * and there's a navaid colocated on the field, return it's db pointer,
      * else return NULL.
      */
{
   DATABASE_INFO *db;
   extern BOOLEAN lookup_desig ();

   if (IS_NAVAID (wp.db->mode)) {
      return (wp.db);
   } else if ((wp.db->mode == WP_AIRPORT) && 
	      (lookup_desig (WP_VIA, wp.db->desig, &db)) &&
	      IS_NAVAID (db->mode) &&
	      COLOCATED (db, wp.db) ) {
      return (db);
   } else {
      return ((DATABASE_INFO*) 0);
   }
}

/*----------------------------------------------------------------------------*/
static track_nav1 ()
     /*
      * for each waypoint, determine what VOR to tune in on NAV1.
      *
      * This is tricky - some waypoints are navaids - so we want to track to 
      * (and sometimes from) them - others are just "distance-since-last-
      * -waypoint" or "lat/long" waypoints, and we can't track to those. 
      * From and To Airports also sometimes have VORS on-field, so we need to 
      * recognize when that happens so that we can track to/from airport 
      * waypoints. 
      *
      * The vor_fix[0] for each waypoint is for the leg _between_ that waypoint
      * and the next one. If there is no navaid that can be used to track along
      * that leg, set the .valid falg to FALSE.
      */
{
   int this, to;
   DATABASE_INFO *db;

   /*
    * invalidate all the current NAV1 settings
    */
   for (this = 0; this < num_waypoints; this++) 
      waypoints[this].vor_fix[0].valid = FALSE;

   to = 0;
   for (this = 0; (to != -1) && (this < num_waypoints); this++) {
      to = next_non_incremental (this+1);
      if (
	  /*
	   * if the next non-incremental waypoint is a navaid (or if an
	   * airport and there's a navaid colocated on the field) then use that 
	   * as NAV1 for this waypoint and those up to that one
	   */
	  ((to >= 0) && 
	   (db = get_navaid (waypoints[to]))) ||
	  /*
	  * else, if this waypoint is a navaid, use it
	  */
	   (db = get_navaid (waypoints[this])) ) {
	 for (; this < to; this++) {
	    waypoints[this].vor_fix[0].valid = TRUE;
	    waypoints[this].vor_fix[0].db    = db;
	 }
	 this = to - 1;
      } 
      /*
       * else, we're out of luck - leave the fix invalid.
       */
   }
}

/*----------------------------------------------------------------------------*/
normalize_heading (head)
     double *head;
{
   if (*head < 0.0)
      *head += 360.0;
   else if (*head > 360.0)
      *head -= 360.0;
}

/*----------------------------------------------------------------------------*/
static BOOLEAN do_fix (plane_heading, latitude, longitude, vor_fix, force_from)
     double plane_heading;
     double latitude;
     double longitude;
     VOR_FIX *vor_fix;
     BOOLEAN force_from;
{
   double d;
   double lat_from  = latitude;
   double long_from = longitude;
   double lat_to    = vor_fix->db->latitude;
   double long_to   = vor_fix->db->longitude;
   
   if (! vor_fix->db)
      return FALSE;

   if ((lat_from == lat_to) && (long_from == long_to))
      return FALSE;

   distance_and_heading (lat_from, long_from, lat_to, long_to,
			 &vor_fix->distance, &vor_fix->heading); 
   vor_fix->heading += vor_fix->db->mag_variation;
   normalize_heading (&vor_fix->heading);
   d = plane_heading - vor_fix->heading;
   vor_fix->from_to = TO;
   
   {
      double anti;
      double d1;
      
      anti = vor_fix->heading - 180;
      normalize_heading( &anti );
      d1 = plane_heading - anti;
      d1 = ( d1 < 0.0 ) ? -d1 : d1;      /* normalize_heading(&d1); */
      d  = ( d  < 0.0 ) ? -d  : d ;      /* normalize_heading(&d); */
      if( force_from || ( d > d1 ) ) {
	 vor_fix->from_to = FROM;
	 vor_fix->heading += 180.0;
      }
   }
   normalize_heading (&vor_fix->heading);
   return TRUE;
}

/*----------------------------------------------------------------------------*/
BOOLEAN compute_plan (auto_nav1)
     BOOLEAN auto_nav1;
{
   int i;
   int num_pairs = 0;
#define MAX_NUM_PAIRS 30
   double total_dist [MAX_NUM_PAIRS];
   double distance, heading;

   /*
    * locate the waypoints specified only by distance from last waypoint:
    */
   locate_incrementals ();

   /*
    * accumulate the total distance between each from/to pair
    */
   num_pairs = 0;
   for (i = 0; i < num_waypoints; i++) {
      if (waypoints[i].wp_kind == WP_FROM) {
	 waypoints[i].eta.valid = TRUE;
	 waypoints[i].eta.value = 0.0;
	 total_dist [num_pairs] = 0.0;
      }
      if ((i+1 < num_waypoints) && (waypoints[i].wp_kind != WP_TO)) {
	 distance_and_heading (waypoints[i].db->latitude,
			       waypoints[i].db->longitude,
			       waypoints[i+1].db->latitude,
			       waypoints[i+1].db->longitude,
			       &distance, &heading);
	 waypoints[i].dist_leg.valid = TRUE;
	 waypoints[i].tc.valid       = TRUE;
	 waypoints[i].mc.valid       = TRUE;
	 waypoints[i].dist_leg.value = distance;
	 waypoints[i].tc.value       = heading;
	 waypoints[i].mc.value       = heading + waypoints[i].db->mag_variation;
	 normalize_heading (&waypoints[i].mc.value);
	 normalize_heading (&waypoints[i].tc.value);
      } else {
	 waypoints[i].dist_leg.valid = FALSE;
	 waypoints[i].tc.valid       = FALSE;
	 waypoints[i].mc.valid       = FALSE;
      }

      waypoints[i].dist.valid = TRUE;
      waypoints[i].dist.value = total_dist [num_pairs];

      total_dist [num_pairs] += waypoints[i].dist_leg.value;

      if (waypoints[i].wp_kind == WP_TO)
	 num_pairs++;
   }

   /*
    * now, do the distance remaining:
    */
   num_pairs = 0.0;
   for (i = 0; i < num_waypoints; i++) {
      waypoints[i].dist_remain.value = 
	 total_dist [num_pairs] - waypoints[i].dist.value;
      waypoints[i].dist_remain.valid = TRUE;
      if (waypoints[i].wp_kind == WP_TO)
	 num_pairs++;
   }

   /*
    * wind correction (heading, ground speed)
    */
   for (i = 0; i < num_waypoints; i++) {
      if (waypoints[i].tas.valid && 
	  waypoints[i].mc.valid) {

	 if (!waypoints[i].wind_speed.valid)
	    waypoints[i].wind_speed.value = 0.0; 
	 if (!waypoints[i].wind_direction.valid)
	    waypoints[i].wind_direction.value = 0.0;
 
	 wind_correct (waypoints[i].tas.value, 
		       waypoints[i].mc.value, 
		       waypoints[i].wind_speed.value,
		       waypoints[i].wind_direction.value,
		       &waypoints[i].egs.value,
		       &waypoints[i].mh.value);

	 waypoints[i].egs.valid  = TRUE;
	 waypoints[i].mh.valid   = TRUE;
	 normalize_heading (&waypoints[i].mh.value);

	 /*
	  * time in hours
	  */
	 waypoints[i].eta_leg.valid = waypoints[i].dist_leg.valid;
	 waypoints[i].eta_leg.value = waypoints[i].dist_leg.value / 
	    waypoints[i].egs.value;
      } else {
	 waypoints[i].egs.valid     = FALSE;
	 waypoints[i].mh.valid      = FALSE;
	 waypoints[i].eta.valid     = FALSE;
	 waypoints[i].eta_leg.valid = FALSE;
      }
      waypoints[i].eta.valid     = (BOOLEAN) (waypoints[i].wp_kind != WP_FROM);
      waypoints[i].eta.value     = (waypoints[i].dist.value == 0.0) ? 0.0 :
	 waypoints[i-1].eta_leg.value + waypoints[i-1].eta.value;
   }

   /*
    * fuel burn:
    */
   for (i = 0; i < num_waypoints; i++) {
      waypoints[i].fuel_leg.valid = (BOOLEAN) (waypoints[i].fuel_rate.valid &&
					       waypoints[i].eta_leg.valid);
      if (waypoints[i].fuel_leg.valid)
	 waypoints[i].fuel_leg.value = 
	    waypoints[i].fuel_rate.value * waypoints[i].eta_leg.value;
      if (waypoints[i].refuel) {
	 waypoints[i].fuel_amt.valid = TRUE;
      } else if (i && (waypoints[i].wp_kind == WP_FROM) &&
		 (!waypoints[i].refuel)) {
	 waypoints[i].fuel_amt.valid = waypoints[i-1].fuel_amt.valid;
	 waypoints[i].fuel_amt.value = waypoints[i-1].fuel_amt.value;
      } else if (i && waypoints[i-1].fuel_leg.valid &&
		 waypoints[i-1].fuel_amt.valid) {
	 waypoints[i].fuel_amt.value = 
	    waypoints[i-1].fuel_amt.value - waypoints[i-1].fuel_leg.value;
      }
      if (waypoints[i].extra_fuel_burn.valid)
	 waypoints[i].fuel_amt.value -= waypoints[i].extra_fuel_burn.value;
   }

   if (auto_nav1)
      track_nav1 ();

   /*
    * VOR cross fixes:
    */
   {
      int vor;
      for (i = 0; i < num_waypoints; i++) {
	 for (vor = 0; vor < MAX_NUM_VOR_FIXES; vor++) {
	    if (waypoints[i].vor_fix[vor].valid) {
	       if (! do_fix (waypoints[i].mh.value, waypoints[i].db->latitude,
			     waypoints[i].db->longitude, 
			     &waypoints[i].vor_fix[vor], FALSE) ) {
		  /*
		   * if we failed to compute an xfix, it could be because we're
		   * right on top of the navaid - pretend we're at the next
		   * waypoint (since that's where we're going...)
		   */
		  if ((i+1) < num_waypoints) {
		     if (!do_fix (waypoints[i].mh.value, 
				  waypoints[i+1].db->latitude,
				  waypoints[i+1].db->longitude, 
				  &waypoints[i].vor_fix[vor], TRUE))
			waypoints[i].vor_fix[vor].valid = FALSE;
		  } else
		     waypoints[i].vor_fix[vor].valid = FALSE;
	       }
	    }
	 }
      }
   }
   return TRUE;
}