/*
    Interpolate filter

    Copyright (C) 2002 Robert Lipe, robertlipe@usa.net

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111 USA

 */

#include "defs.h"
#include "filterdefs.h"
#include "grtcirc.h"

#if FILTERS_ENABLED
#define MYNAME "Interpolate filter"

static char* opt_interval = NULL;
int interval = 0;
static char* opt_dist = NULL;
double dist = 0;
static char* opt_route = NULL;

static
arglist_t interpfilt_args[] = {
  {
    "time", &opt_interval, "Time interval in seconds", NULL,
    ARGTYPE_BEGIN_EXCL | ARGTYPE_BEGIN_REQ | ARGTYPE_INT,
    "0", NULL
  },
  {
    "distance", &opt_dist, "Distance interval in miles or kilometers",
    NULL, ARGTYPE_END_EXCL | ARGTYPE_END_REQ | ARGTYPE_STRING,
    ARG_NOMINMAX
  },
  {
    "route", &opt_route, "Interpolate routes instead", NULL,
    ARGTYPE_BOOL, ARG_NOMINMAX
  },
  ARG_TERMINATOR
};

void
interpfilt_process(void)
{
  queue* backuproute = NULL;
  queue* elem, *tmp, *elem2, *tmp2;
  route_head* rte_new;
  int count = 0;
  int first = 0;
  double lat1 = 0, lon1 = 0;
  int time1 = 0;
  int timen;
  double distn;
  double curdist;
  double rt1, rn1, rt2, rn2;

  if (opt_route) {
    route_backup(&count, &backuproute);
    route_flush_all_routes();
  } else {
    track_backup(&count, &backuproute);
    route_flush_all_tracks();
  }
  QUEUE_FOR_EACH(backuproute, elem, tmp) {
    route_head* rte_old = (route_head*)elem;

    rte_new = route_head_alloc();
    rte_new->rte_name = xstrdup(rte_old->rte_name);
    rte_new->rte_desc = xstrdup(rte_old->rte_desc);
    rte_new->fs = fs_chain_copy(rte_old->fs);
    rte_new->rte_num = rte_old->rte_num;
    if (opt_route) {
      route_add_head(rte_new);
    } else {
      track_add_head(rte_new);
    }
    first = 1;
    QUEUE_FOR_EACH(&rte_old->waypoint_list, elem2, tmp2) {
      waypoint* wpt = (waypoint*)elem2;
      if (first) {
        first = 0;
      } else {
        if (opt_interval &&
            wpt->creation_time - time1 > interval) {
          for (timen = time1+interval;
               timen < wpt->creation_time;
               timen += interval) {
            waypoint* wpt_new = waypt_dupe(wpt);
            wpt_new->creation_time = timen;
            if (wpt_new->shortname) {
              xfree(wpt_new->shortname);
            }
            if (wpt_new->description) {
              xfree(wpt_new->description);
            }
            wpt_new->shortname = wpt_new->description = NULL;
            linepart(lat1, lon1,
                     wpt->latitude, wpt->longitude,
                     (double)(timen-time1)/
                     (double)(wpt->creation_time-time1),
                     &wpt_new->latitude,
                     &wpt_new->longitude);
            if (opt_route) {
              route_add_wpt(rte_new, wpt_new);
            } else {
              track_add_wpt(rte_new, wpt_new);
            }
          }
        } else if (opt_dist) {
          rt1 = RAD(lat1);
          rn1 = RAD(lon1);
          rt2 = RAD(wpt->latitude);
          rn2 = RAD(wpt->longitude);
          curdist = gcdist(rt1, rn1, rt2, rn2);
          curdist = radtomiles(curdist);
          if (curdist > dist) {
            for (distn = dist;
                 distn < curdist;
                 distn += dist) {
              waypoint* wpt_new = waypt_dupe(wpt);
              wpt_new->creation_time = distn/curdist*
                                       (wpt->creation_time - time1) + time1;
              if (wpt_new->shortname) {
                xfree(wpt_new->shortname);
              }
              if (wpt_new->description) {
                xfree(wpt_new->description);
              }
              wpt_new->shortname = wpt_new->description = NULL;
              linepart(lat1, lon1,
                       wpt->latitude, wpt->longitude,
                       distn/curdist,
                       &wpt_new->latitude,
                       &wpt_new->longitude);
              if (opt_route) {
                route_add_wpt(rte_new, wpt_new);
              } else {
                track_add_wpt(rte_new, wpt_new);
              }
            }
          }
        }
      }
      if (opt_route) {
        route_add_wpt(rte_new, waypt_dupe(wpt));
      } else {
        track_add_wpt(rte_new, waypt_dupe(wpt));
      }

      lat1 = wpt->latitude;
      lon1 = wpt->longitude;
      time1 = wpt->creation_time;
    }
  }
  route_flush(backuproute);
  xfree(backuproute);
}

void
interpfilt_init(const char* args)
{

  char* fm;
  if (opt_interval && opt_dist) {
    fatal(MYNAME ": Can't interpolate on both time and distance.\n");
  } else if (opt_interval && opt_route) {
    fatal(MYNAME ": Can't interpolate routes on time.\n");
  } else if (opt_interval) {
    interval = atoi(opt_interval);
  } else if (opt_dist) {
    dist = strtod(opt_dist, &fm);
    if ((*fm == 'k') || (*fm == 'K')) {
      /* distance is kilometers, convert to miles */
      dist *= .6214;
    }
  } else {
    fatal(MYNAME ": No interval specified.\n");
  }
}

filter_vecs_t interpolatefilt_vecs = {
  interpfilt_init,
  interpfilt_process,
  NULL,
  NULL,
  interpfilt_args
};
#endif // FILTERS_ENABLED
