/***********************************************************************
**
**   cucalc.cpp
**
**   This file is part of Cumulus.
**
************************************************************************
**
**   Copyright (c):  2002 by André Somers
**
**   This file is distributed under the terms of the General Public
**   Licence. See the file COPYING for more information.
**
**   $Id: cucalc.cpp,v 1.93 2005/02/23 13:55:38 andre Exp $
**
***********************************************************************/

#include <stdlib.h>
#include <cmath>
#include "generalconfig.h"
#include "cucalc.h"
#include "mapcalc.h"
#include "gpsnmea.h"
#include "mapmatrix.h"
#include "windanalyser.h"
#include "reachablelist.h"
#include "altimetermodedialog.h"


#define MAX_MCCREADY 10.0
#define MAX_SAMPLECOUNT 300

CuCalc * calculator;
extern MapContents * _globalMapContents;

CuCalc::CuCalc(QObject* parent)
: QObject(parent)
{
  GeneralConfig *conf = GeneralConfig::instance();

  manualAltitude.setMeters( conf->getManualNavModeAltitude() );
  lastAltitude = manualAltitude;  // provide support to config altitude in settings
  lastAGLAltitude = manualAltitude;
  lastGNSSAltitude = manualAltitude;
  lastAGLAltitudeError.setMeters(0);

  lastETA=QTime(0,0);
  lastBearing=-1;
  lastHeading=-1;
  lastDistance=-1;
  selectedWp=0;
  lastMc = 0.0;
  _polar = NULL;
  _vario = new Vario (this);
  samplelist = new LimitedList <flightsample> (MAX_SAMPLECOUNT);
  _windAnalyser = new WindAnalyser(this);
  _reachablelist = new ReachableList(this),
  _windStore = new WindStore(this);
  lastFlightMode=unknown;
  _marker=0;
  _glider=0;
  _pastFirstFix=false;
  _altimeter_mode = AltimeterModeDialog::mode();
  // hook up the backend components
  connect (_vario, SIGNAL(newVario(const Speed&)),this,SLOT(slot_Variometer(const Speed&)));
  connect (this, SIGNAL(newSample()), _windAnalyser, SLOT(slot_newSample()));
  connect (this, SIGNAL(flightModeChanged(CuCalc::flightmode, int)), _windAnalyser, SLOT(slot_newFlightMode(CuCalc::flightmode, int)));
  connect (_windAnalyser, SIGNAL(newMeasurement(Vector, int)), _windStore, SLOT(slot_measurement(Vector, int)));
  connect (_windStore, SIGNAL(newWind(Vector&)), this, SLOT(slot_Wind(Vector&)));
  connect (this, SIGNAL(newAltitude(const Altitude&)), _windStore, SLOT(slot_Altitude()));

  // make internal connection so the flightModeChanged signal is re-emitted with the marker value
  connect (this, SIGNAL(flightModeChanged(CuCalc::flightmode)), this, SLOT(_slot_flightModeChanged(CuCalc::flightmode)));
}

CuCalc::~CuCalc()
{
  delete samplelist;

  if( _glider )
    delete _glider;
}

/** Read property of QTime lastETA. */
const QTime& CuCalc::getlastETA(){
	return lastETA;
}

/** Read property of int lastBearing. */
const int& CuCalc::getlastBearing(){
	return lastBearing;
}

/** Read property of int lastWind. */
const Vector& CuCalc::getlastWind(){
	return lastWind;
}

/** Read property of double lastDistance. */
const Distance& CuCalc::getlastDistance(){
	return lastDistance;
}

/** Read property of wayPoint* selectedWp. */
const wayPoint* CuCalc::getselectedWp(){
	return selectedWp;
}

/** Read property of Altitude lastAltitude. */
const Altitude& CuCalc::getlastAltitude(){
	return lastAltitude;
}

/** Read property of Altitude lastAltitude. */
const Altitude& CuCalc::getlastAGLAltitude(){
  lastAGLAltitude=lastAltitude - lastElevation;
  return lastAGLAltitude;
}

/** Read property of Altitude lastAltitude. */
const Altitude& CuCalc::getlastGNDAltitude(){
  if ( selectedWp ) {
    return lastAltitude - selectedWp->elevation;
  } else {
    return lastAltitude;
  }
}

/** Read property of Altitude for Altimeter display */
const Altitude& CuCalc::getAltimeterAltitude(){
  if( _altimeter_mode == 1 )  // GND Mode
    return lastAGLAltitude;
  else
    return lastAltitude;
}

const AltitudeCollection& CuCalc::getAltitudeCollection(){
  lastAltCollection.gndAltitude=lastAGLAltitude;
  lastAltCollection.gpsAltitude=lastAltitude; 
  lastAltCollection.pressureAltitude=lastAltitude;
  lastAltCollection.gpsAltitudeError=0;
  lastAltCollection.gndAltitudeError=lastAGLAltitudeError;

  return lastAltCollection;
}

/** Read property of glider polar  */
Polar * CuCalc::getPolar(){
	return _polar;
}
 
/** Read property of Speed lastSpeed. */
Speed& CuCalc::getlastSpeed(){
	return lastSpeed;
}

/** Read property of Speed lastBestSpeed. */
Speed& CuCalc::getlastBestSpeed(){
	return lastBestSpeed;
}

/** Read property of last glide path */
const Altitude& CuCalc::getlastGlidePath()
{
  return lastGlidePath;
}

/** Read property of McCready setting */
const Speed& CuCalc::getlastMc()
{
  return lastMc;
}

/** Read property of QPoint lastPosition. */
const QPoint& CuCalc::getlastPosition(){
	return lastPosition;
}

/** Read property of int lastHeading. */
int CuCalc::getlastHeading(){
	return lastHeading;
}

/** called on altitude change */
void CuCalc::slot_Altitude(){
  lastAltitude=gps->getLastAltitude();
  lastGNSSAltitude=gps->getLastGNSSAltitude();
  lastAGLAltitude = lastAltitude - Altitude( _globalMapContents->findElevation(lastPosition, &lastAGLAltitudeError) );
  emit newAltitude(lastAltitude);
  calcGlidePath();
  // qDebug("slot_Altitude");
}

/** Called if a new heading has been obtained */
void CuCalc::slot_Heading(){
  // qDebug("lastSpeed=%f m/s", lastSpeed.getMps());
  if( lastSpeed.getMps() <= 0.2 )
    {
      // @AP: don't forward values, when the speed is nearly
      // zero. Arrow will stay in last position. Same does make
      // Garmin-Pilot III
      return;
    }
  lastHeading = (int)gps->getLastHeading();
  emit newHeading(lastHeading);
  // if we have no bearing, lastBearing is -1;
  // this is only a small mistake, relBearing points to north
  emit newRelBearing (lastBearing - lastHeading);
}

/** called if a new speed fix has been received */
void CuCalc::slot_Speed(){
  lastSpeed=gps->getLastSpeed();
   emit newSpeed(lastSpeed);
}

/** called if a new position-fix has been established. */
void CuCalc::slot_Position(){
  lastPosition=gps->getLastCoord();
  lastElevation = Altitude( _globalMapContents->findElevation(lastPosition, &lastElevationError) );
  emit newPosition(lastPosition);
  calcDistance();
  calcBearing();
  calcETA();
  calcGlidePath();
    // Calculate List of rechable items
  _reachablelist->calculate(false);
}

/** Called if a new waypoint has been selected. */
void CuCalc::slot_WaypointChange(wayPoint * newWp)
{
  selectedWp=newWp;

  // @ee the new waypoint may be null !
  if (newWp)
    {
      emit newWaypoint(newWp->name);
    }
  else
    {
      // @AP: emit signal, that waypoint display is removed
      emit newWaypoint("");
    }

  calcDistance();
  calcBearing();
  calcETA();
  calcGlidePath();
  // for debug purpose trigger manually a calculation by selecting a waypoint
}

/**
 * Called if a waypoint has to be deleted.
 */
void CuCalc::slot_WaypointDelete(wayPoint * newWp)
{
  // @AP: check, if waypoint to be deleted is selected. In this case a
  // deselection must be done
  if( selectedWp && selectedWp->origP == newWp->origP )
    {
      selectedWp=0;
      calcDistance();
      calcBearing();
      calcETA();
      calcGlidePath();
    }
}

/** Calculates the distance to the currently selected waypoint and emits newDistance if the distance has changed. */
void CuCalc::calcDistance(){
  Distance result;

  if (selectedWp==0) {
    result=-1;
  } else {

    result.setKilometers(dist(double(lastPosition.x()),double(lastPosition.y()),selectedWp->origP.lat(),selectedWp->origP.lon()));
  }

  if (result!=lastDistance) {
    lastDistance=result;
    emit newDistance(lastDistance);
  }
}

/** Calculates the ETA (Estimated Time to Arrival) to the current waypoint and emits a signal newETA if the value has changed. */
void CuCalc::calcETA()
{
  QTime etaNew(0, 0);

  // qDebug("lastSpeed=%f m/s", lastSpeed.getMps());

  if( !selectedWp || lastSpeed.getMps() <= 0.2 || ! gps->getConnected() )
    {
      if( ! lastETA.isNull() )
	{
	  emit newETA(etaNew);
	  lastETA = etaNew;
	}

      return;
    }

  int eta = (int) (lastDistance.getMeters() / lastSpeed.getMps());

  if( eta > 99*3600 )
    {
      // Don't emit times greater as 99 hours. ETA will be set to
      // undefined in such a case to avoid a problem in the map
      // display with to large values.
      emit newETA(etaNew);
    }

  etaNew = etaNew.addSecs(eta);

  if( lastETA.hour() == etaNew.hour() &&
      lastETA.minute() == etaNew.minute() )
    {
      // changes in seconds only will be ignored
      return;
    }

  if( etaNew != lastETA )
    {
      lastETA = etaNew;
      emit newETA(lastETA);
      // qDebug("New ETA=%s", etaNew.toString().latin1());
    }
}

/** Calculates the glide path to the current waypoint and the needed height */

bool CuCalc::glidePath(int aLastBearing, Distance aDistance, Altitude aElevation, Altitude &arrivalAlt, Speed &bestSpeed )
{
  if (!_polar) {
    arrivalAlt.setInvalid();
    bestSpeed.setInvalid();
    return false;
  }
  
  // we use the method described by Bob Hansen
  // get best speed for zero wind V0
  Speed speed = _polar->bestSpeed(0.0, 0.0, lastMc);
  //qDebug ("rough best speed: %f", speed.getKph());
    
  // wind has a negative vector!
  //qDebug ("wind: %d/%f", lastWind.getAngleDeg(), lastWind.getSpeed().getKph());

  // assume we are heading for the wp
  Vector groundspeed (aLastBearing, speed);
  //qDebug ("groundspeed: %d/%f", groundspeed.getAngleDeg(), groundspeed.getSpeed().getKph());

  // we add wind because of the negative direction
  Vector airspeed = groundspeed + lastWind;
  //qDebug ("airspeed: %d/%f", airspeed.getAngleDeg(), airspeed.getSpeed().getKph());

  // this is the first iteration of the Bob Hansen method
  Speed headwind = groundspeed.getSpeed() - airspeed.getSpeed() ;
  //qDebug ("headwind: %f", headwind.getKph());
  
  Altitude givenAlt (lastAltitude - Altitude (aElevation) - _minimalArrival);

  // improved speed for wind V1
  speed = _polar->bestSpeed(headwind, 0.0, lastMc);
  //qDebug ("improved best speed: %f", speed.getKph());
  bestSpeed = speed;
  // the ld is over ground, so we take groundspeed
  double ld =_polar->bestLD(speed, groundspeed.getSpeed(), 0.0);

  arrivalAlt = (givenAlt - (aDistance / ld));

  //qDebug ("ld = %f", ld);
  //qDebug ("bestSpeed: %f", speed.getKph());
//  qDebug ("lastSpeed: %f", lastSpeed.getKph());
//  qDebug ("above: %f", above.getMeters());
  return true;
}


void CuCalc::calcGlidePath ()
{
  //  qDebug ("CuCalc::calcGlidePath");
  Speed speed;
  Altitude above;
  // calculate new glide path
  if (!selectedWp)
       return;
  glidePath(lastBearing, lastDistance, selectedWp->elevation, above, speed );
  if (speed != lastBestSpeed)
  {
    lastBestSpeed = speed;
    emit bestSpeed (speed);
  }
  if (above != lastGlidePath)
  {
    lastGlidePath = above;
    emit glidePath (above);
  }
}

/** Calculates the bearing to the currently selected waypoint, and emits signal newBearing if the bearing has changed. */
void CuCalc::calcBearing(){

  int iresult;
  int lH=lastHeading;
  if (lH == -1) lH=0;
  
  if (selectedWp==0)
  {
    iresult=-1;
    if (iresult!=lastBearing)
    {
      lastBearing=iresult;
      emit newBearing(-1000);
      // if we have no waypoint, let the relbearing arrow point to north
      emit newRelBearing (-lH);
    }
  }
  else
  {
    double result=getBearing(lastPosition, selectedWp->origP);
    iresult=int(result * 57.2958);
    if (iresult!=lastBearing)
    {
      lastBearing=iresult;
      emit newBearing(lastBearing);
      emit newRelBearing (lastBearing - lH);
    }
  }
}

/** Called if the position is changed manually. */
void CuCalc::slot_changePosition(int direction){
  lastAltitude = manualAltitude;  // provide support to config altitude in settings
  lastAGLAltitude = lastAltitude - Altitude( _globalMapContents->findElevation(lastPosition) );
  emit newAltitude(lastAltitude);
  
  extern MapMatrix * _globalMapMatrix;

  double distance=_globalMapMatrix->getScale(MapMatrix::CurrentScale)*10;
  double kmPerDeg;
  
  switch(direction)
    {
      case MapMatrix::North:
        kmPerDeg=dist(lastPosition.x(), lastPosition.y(), lastPosition.x()+600000, lastPosition.y());
        lastPosition=QPoint((int) rint(lastPosition.x()+(distance/kmPerDeg) * 600), lastPosition.y());
        break;
      case MapMatrix::West:
        kmPerDeg=dist(lastPosition.x(), lastPosition.y(), lastPosition.x(), lastPosition.y()+600000);
        lastPosition=QPoint(lastPosition.x(), (int) rint(lastPosition.y()-(distance/kmPerDeg) * 600));
        break;
      case MapMatrix::East:
        kmPerDeg=dist(lastPosition.x(), lastPosition.y(), lastPosition.x(), lastPosition.y()+600000);
        lastPosition=QPoint(lastPosition.x(), (int) rint(lastPosition.y()+(distance/kmPerDeg) * 600));
        break;
      case MapMatrix::South:
        kmPerDeg=dist(lastPosition.x(),lastPosition.y(), lastPosition.x()+600000,lastPosition.y());
        lastPosition=QPoint((int) rint(lastPosition.x()-(distance/kmPerDeg) * 600), lastPosition.y());
        break;
      case MapMatrix::Home:
        lastPosition=QPoint(_globalMapMatrix->getHomeCoord());
        break;
      case MapMatrix::Waypoint:
        if (selectedWp!=0) lastPosition=selectedWp->origP;
        break;
    }

  lastElevation = Altitude( _globalMapContents->findElevation(lastPosition, &lastElevationError) );
  emit newPosition(lastPosition);
  
  lastAltitude = manualAltitude;  // provide support to config altitude in settings
  lastAGLAltitude = lastAltitude - Altitude( _globalMapContents->findElevation(lastPosition) );
  emit newAltitude(lastAltitude);
  
  calcDistance();
  calcBearing();
  calcGlidePath();  
  // calculate always when moving manually (big delta !)
  _reachablelist->calculate(true);
  //qDebug("Elevation: %d m",_globalMapContents->findElevation(lastPosition) );
}


/** No descriptions */
void CuCalc::slot_changePositionN(){
  slot_changePosition(MapMatrix::North);
}

/** No descriptions */
void CuCalc::slot_changePositionS(){
  slot_changePosition(MapMatrix::South);
}

/** No descriptions */
void CuCalc::slot_changePositionE(){
  slot_changePosition(MapMatrix::East);
}

/** No descriptions */
void CuCalc::slot_changePositionW(){
  slot_changePosition(MapMatrix::West);
}

/** No descriptions */
void CuCalc::slot_changePositionHome(){
  slot_changePosition(MapMatrix::Home);
}

/** No descriptions */
void CuCalc::slot_changePositionWp(){
  slot_changePosition(MapMatrix::Waypoint);
}

/** increment McCready value */
void CuCalc::slot_McUp()
{ if (lastMc.getMps() <= MAX_MCCREADY - 0.5)
  {
    lastMc.setMps(lastMc.getMps()+0.5);
    calcGlidePath();
    emit newMc (lastMc);
  }
}

/** set McCready value */
void CuCalc::slot_Mc(const Speed& spd)
{
  lastMc.setMps(spd.getMps());
  calcGlidePath();
  emit newMc (lastMc);
}

/** decrement McCready value; don't get negative */
void CuCalc::slot_McDown()
{
  if (lastMc.getMps() >= 0.5)
  {
    lastMc.setMps(lastMc.getMps()-0.5);
    calcGlidePath();
    emit newMc (lastMc);
  }
}

void CuCalc::slot_Variometer (const Speed& lift)
{
  if (lastVario != lift)
  {
    lastVario = lift;
    emit newVario (lift);
  }
}

/** Sets the current position to point newPos. */
void CuCalc::setPosition(const QPoint& newPos)
{
  lastPosition=newPos;
  emit newPosition(lastPosition);

  calcDistance();
  calcBearing();
  calcETA();
  calcGlidePath();
}

/** Sets the glider type to allow calculation of polar data */
void CuCalc::slot_settingschanged ()
{
  GeneralConfig *conf = GeneralConfig::instance();

  double arrival = conf->getSafetyAltitude().getMeters();
  if (arrival >= 0.0 )
    _minimalArrival.setMeters(arrival);
 
  _altimeter_mode = AltimeterModeDialog::mode(); 
  emit newAltitude(lastAltitude);  // show initial altitude for manual mode
  // qDebug("Settings changed %d",_altimeter_mode );
}

/** This slot is called by the NMEA interpreter if a new fix has been received.  */
void CuCalc::slot_newFix(){
  //before we start making samples, let's be sure we have all the data we need for that. So, we wait for the second Fix.
  if (!_pastFirstFix) {
    _pastFirstFix=true;
    return;
  }

  //create a new sample struct
  flightsample * sample=new flightsample;

  //fill it with the relevant data
  sample->time=gps->getLastTime();
  sample->altitude.setMeters(lastAltitude.getMeters());
  sample->GNSSAltitude.setMeters(lastGNSSAltitude.getMeters());
  sample->position=lastPosition;
/*  sample->vector.setMps(lastSpeed.getMps());
  sample->vector.setAngle(lastHeading);*/
  sample->vector.setAngleAndSpeed(lastHeading, lastSpeed);

  //qDebug("Speed in sample: %d kph", int(lastSpeed.getKph()));
  //qDebug("Direction in sample: %d degrees", int(lastHeading));
  Vector groundspeed (lastHeading, lastSpeed);
  // qDebug ("groundspeed: %d/%f", groundspeed.getAngleDeg(), groundspeed.getSpeed().getKph());
  // qDebug ("wind: %d/%f", lastWind.getAngleDeg(), lastWind.getSpeed().getKph());
  Vector airspeed = groundspeed + lastWind;
  // qDebug ("airspeed: %d/%f", airspeed.getAngleDeg(), airspeed.getSpeed().getKph());
  if( lastWind.getSpeed().getKph() != 0 )
     sample->airspeed = airspeed.getSpeed();
 
  //add to the samplelist
  samplelist->add(sample);
  lastSample = sample;
  // call vario calculation
  _vario->newAltitude();

  //start analysing...
  //determine if we are standing still, cruising, circling or doing something else
  determineFlightStatus();

  //let the world know we have added a new sample to our samplelist
  emit newSample();

}

/** Determines the status of the flight: unkown, standstill, cruising, circlingL, circlingR */
void CuCalc::determineFlightStatus(){
  /*
  WARNING: THIS CODE IS HIGHLY EXPERIMENTAL AND UNTESTED IN FLIGHT!
  
  In principle, we are determining the flightstatus from the last 20 seconds of flight. We may
  need to adjust this, as this is highly experimental code. However, if we allready have a
  flightstatus, we only need to check if it has changed. If that is the case, we set the status
  to "Unkown" so the next fix will trigger a normal analysation again.

  Further optimisations could include using the airspeed to determine the flightmode. This however
  requires a good working windestimation, wich requires a good working flightstatus.... etc.
  Another approch would be to keep a counter that could allow for, say, one or two 'errors' in the
  data before a flightmode is changed to 'unknown'.
  
  */
  /*
    We define a maximum turnrate of 10 degrees per second. This should allow for course-corrections.
    This can be a bit more advanced to allow for temporary changes in circling speed in order to better
    center a thermal.
  */
  #define MAXCRUISEANGDIFF 8 //see above
   /*
    We define a minimum turnrate of 4 degrees per second (that is, one turn takes at most one and a half minute).

    This can be a bit more advanced to allow for temporary changes in circling speed in order to better
    center a thermal.
  */
  #define MINTURNANGDIFF 4 //see above
  /*
    We define a maximum altitude drift to refine the detection of a standstill. Because GPS altitudes tend to drift,
    we can't rely on the exact same altitude over any length of time. We need to allow for some drift.
  */
  #define MAXALTDRIFT 10 //see above
  /*
    We define an analysis timeframe. This timeframe determines how far back we're going in history (in seconds)
    for our analysis. Note that the actual timedifference between the first and the last sample used may
    differ from this timeframe.
  */
  #define TIMEFRAME 10
 
  if (samplelist->count()<5) return; //we need to have some samples in order to be able to analyse anything.
  
  flightmode newFlightMode=unknown;

  //get headings from the last two samples
  int lastHead=samplelist->at(0)->vector.getAngleDeg();
  int prevHead=samplelist->at(1)->vector.getAngleDeg();
  
  //get the timedifference between these samples
  int timediff=samplelist->at(1)->time.secsTo(samplelist->at(0)->time);
  if (timediff==0) return; //if the timedifference is 0, return (just to be sure). This will only cause problems...

  
  //we are not doing a full analysis if we allready have a flightmode. It suffices to check some
  //basic criteria.
  //lastFlightMode=unknown; //force complete evaluation for now.
  //qDebug("Anglediff: %d",angleDiff(lastHead, prevHead));

  switch (lastFlightMode) {
  case standstill: //we are not moving at all!
    if (samplelist->at(0)->position == samplelist->at(1)->position) {  //may be too ridgid, GPS errors could cause problems here
      return; //no change in flightmode
    } else {
      newFlightMode=unknown;
    }
    break;

  case wave: //we are not moving at all, except vertically!  Needs lots of tweaking and testing...
    if (samplelist->at(0)->position == samplelist->at(1)->position) {  //may be too ridgid, GPS errors could cause problems here
      return; //no change in flightmode
    } else {
      newFlightMode=unknown;
    }
    break;

/*
  case cruising: //we are flying from point A to point B
    if (abs(angleDiff(lastHead, prevHead)) <  (MINTURNANGDIFF * timediff) &&
       samplelist->at(0)->vector.getSpeed().getMps()>5) {
      return; //no change in flightmode
    } else {
      //more detailed check is needed. This is not done by default, because it costs more processing power.
      if (abs(angleDiff(lastHead, samplelist->at(4)->vector.getAngleDeg())) <
            MINTURNANGDIFF * samplelist->at(4)->time.secsTo(samplelist->at(0)->time) &&
          dist(&samplelist->at(4)->position,&samplelist->at(0)->position)*1000 <
            5*samplelist->at(4)->time.secsTo(samplelist->at(0)->time)) {
        newFlightMode=unknown;
        break;
      } else {
        return;
      }
    }
 */
  case cruising: //we are flying from point A to point B
    if (abs(angleDiff(lastHead, _cruiseDirection)) <  MAXCRUISEANGDIFF &&
       samplelist->at(0)->vector.getSpeed().getMps()>5) {
      return; //no change in flightmode
    } else {
      newFlightMode=unknown;
      break;
    }

  case circlingL:
  //turning left means: the heading is decreasing
    if (angleDiff(prevHead, lastHead) > (-MINTURNANGDIFF * timediff)) {
      newFlightMode=unknown;
      break;
    } else {
      return; //no change in flightmode
    }

         
  case circlingR:
  //turning right means: the heading is increasing
    if (angleDiff(prevHead, lastHead) < (MINTURNANGDIFF * timediff)) {
      newFlightMode=unknown;
      break;
    } else {
      return; //no change in flightmode
    }

  default:
    newFlightMode=unknown;
  }
    
  if (newFlightMode==unknown) {
    //we need some real analysis
    QTime tmp= samplelist->at(0)->time.addSecs(-TIMEFRAME); //reference time
    uint ls=1;
    while((samplelist->at(ls)->time>tmp) && (ls<samplelist->count()-1)) ls++;
    //ls now contains the index of the oldest sample we will use for this analysis.
    //Newer samples have lower indices!

    //initialize some values we will be needing...
    bool mayBeL=true;     //this may be a left turn (that is, no big dirchange to the right)
    bool mayBeR=true;     //this may be a right turn (that is, no big dirchange to the left)
    int totalDirChange=0;  //total headingchange. If cruising, this will be low, if turning, it will be high
    int maxSpeed=0;        //maximum speed obtained in this set of samples
    int aDiff=0;                 //difference in heading between two samples
    int totalAltChange=0;  //total change in altitude (absolute)
    int maxAltChange=0;  //maximum change of altitude between samples.
    int altChange=0;
    bool break_analysis=false; //flag to indicate we can stop further analyisis.


    //loop through the samples to get some basic data we can use to distinguish flightmodes
    for (int i=ls-1;i>=0;i--) {
      aDiff=angleDiff(samplelist->at(i+1)->vector.getAngleDeg(),samplelist->at(i)->vector.getAngleDeg());
      //qDebug("analysis: angle1=%d, angle2=%d, diff=%d",int(samplelist->at(i+1)->vector.getAngleDeg()),int(samplelist->at(i)->vector.getAngleDeg()), aDiff);
      //qDebug("analysis: position=(%d, %d)", samplelist->at(i)->position.x(),samplelist->at(i)->position.y() );
      totalDirChange += abs(aDiff);
      maxSpeed=int(MAX(maxSpeed,samplelist->at(i)->vector.getSpeed().getKph()));
      altChange = int(samplelist->at(i)->altitude.getMeters()-samplelist->at(i+1)->altitude.getMeters());
      totalAltChange += altChange;
      maxAltChange = int(MAX(abs(altChange),maxAltChange));
      
      if (aDiff >  MINTURNANGDIFF) mayBeL=false;
      if (aDiff < -MINTURNANGDIFF) mayBeR=false;
      //qDebug("  analysis: sample: %d, speed: %f",i,samplelist->at(i)->vector.getKph());
    }
    //qDebug ("analysis: aDiff=%d, totalDirChange=%d, maxSpeed=%d, totalAltChange=%d, maxAltChange=%d, maybeLeft=%d, maybeRight=%d",aDiff, totalDirChange, maxSpeed, totalAltChange, maxAltChange, int(mayBeL), int(mayBeR));

    //try standstill. We are using a value > 0 because of possible GPS errors.
    /*
       The detection of standstills may be extended further by checking if the altitude matches the terrain altitude. If not
       (or no where near), we can not assume a standstill. This is probably wave flying.
    */
    if (maxSpeed<10) { // if we get under 10 kph for maximum speed, we may be standing still
      if (abs(totalAltChange) * 2 <= MAXALTDRIFT && maxAltChange <= MAXALTDRIFT) {  //check if we had any significant altitude changes
        newFlightMode=standstill;
        break_analysis=true;
      } else {
        newFlightMode=wave;
        break_analysis=true;
      }
    }

    if (!break_analysis) {
      //get the timedifference between the first and the last sample. This might not be the 20 secs we were planning to use at all!
      timediff= samplelist->at(ls-1)->time.secsTo(samplelist->at(0)->time);

      //see if we might be cruising...
      if (mayBeL && mayBeR) { //basicly, we have been going (almost) strait it seems...
        if (totalDirChange < 2 * timediff) {
           //qDebug("analysis: distance=%f m, timedifference=%d s",dist(&samplelist->at(ls-1)->position,&samplelist->at(0)->position)*1000,timediff);
           if (dist(&samplelist->at(ls-1)->position,&samplelist->at(0)->position)*1000>5*timediff) //our average speed should be at least 5 m/s to qualify for cruising
             newFlightMode=cruising;
             _cruiseDirection=samplelist->at(0)->vector.getAngleDeg();
             qDebug("Cruise direction: %d.",_cruiseDirection);
        }
        break_analysis=true;
       }
     }

    if (!break_analysis) {
    //So, we are not standing still, nor are we cruising. Circling then maybe?
    if ((MINTURNANGDIFF * timediff ) < totalDirChange) { //if circling, we should have a minimum average turnrate of MINTURNANGDIFF degrees per second
      if (mayBeL && !mayBeR) newFlightMode=circlingL; 
      if (mayBeR && !mayBeL) newFlightMode=circlingR;
       //we have a new mode: we are circling! If neither of the above are true, we are doing something
       //funny, and the flightmode 'unkown' still applies, so we may also quit analysing...
    }  
   }
  }

  if (newFlightMode!=lastFlightMode) {
    lastFlightMode=newFlightMode;
    samplelist->at(0)->marker=++_marker;
    // qDebug("new flightmode: %d",lastFlightMode);
    emit flightModeChanged(newFlightMode);
  }
}

/** Called if the status of the GPS changes. */
void CuCalc::slot_GpsStatus(GPSNMEA::connectedStatus){
  // qDebug("connection status changed...");
  flightmode newFlightMode=unknown;
  // qDebug("new flightmode=%d, last flightmode=%d",newFlightMode, lastFlightMode);

  if (newFlightMode!=lastFlightMode) {
    lastFlightMode=newFlightMode;
    samplelist->at(0)->marker=++_marker;
    // qDebug("new flightmode: %d",lastFlightMode);
    emit flightModeChanged(newFlightMode);
  } 
}

CuCalc::flightmode CuCalc::currentFlightMode(){
  return lastFlightMode;
}

/** This slot is used internally to re-emit the flightmode signal with the marker value */
void CuCalc::_slot_flightModeChanged(CuCalc::flightmode fm) {
  emit flightModeChanged(fm, _marker);
}

/** Called if the wind measurement changes */
void CuCalc::slot_Wind(Vector& v){
  lastWind = v;
  emit newWind(v);
}

/** Read property of Glider glider. */
Glider * CuCalc::glider() const {
  return _glider;
}

QString CuCalc::gliderType () const
{
  if (_glider)
    return _glider->type();
  else
    return QString::null;
}

/** Write property of Glider glider. */
void CuCalc::setGlider(Glider * _newVal)
{
  if (_glider)
    {
      delete _glider;
      _glider = 0;
    }
  
  // qDebug("CuCalc::setGlider");

  if (_newVal) {
    _glider = _newVal;
    _polar = _glider->polar();
    calcETA();
    calcGlidePath();
    // _reachablelist->calculateFullList();
  }
}

void CuCalc::newSites(void){
  _reachablelist->calculateFullList();
}

