// wptList.c  DOUBLY LINKED LIST
//
//  If legMeters to nextPtr wayPoint is greater than maxLegMeters
//       then insert additional wayPoint(s) to allow time for transit
//
//       loop thru list, add wayPoints when needed, until all legMeters <= maxLegMeters
//


#include <stdio.h>
#include <stdlib.h>
#include "wptList.h"
#include "calcs.h"
#include <math.h>

#define DEGREES_TO_RADS (3.1415926536 / 180)

extern double maxLegMeters;

double big_legMeters = 0.0;      // biggest transit distance in any node of list

extern double missionSpeed;
extern int LAUNCH_OFFSET;
extern double missionTime;
extern int gpsHits;
extern double diveAngle;

extern double surveyLineMeters;  // added 28 Sept 14

void insertLatLonBeg(double latIn, double lonIn)
{
  struct node *newNodePtr;
  newNodePtr=(struct node *)malloc(sizeof(struct node));
  newNodePtr->Lat=latIn;
  newNodePtr->Lon=lonIn;
  if(headNodePtr==NULL)
  {
    newNodePtr->prevPtr=newNodePtr->nextPtr=NULL;
    headNodePtr=newNodePtr;
    tailNodePtr=newNodePtr;
  }
  else
  {
    newNodePtr->nextPtr=headNodePtr;
    newNodePtr->prevPtr=NULL;
    headNodePtr->prevPtr=newNodePtr;
    headNodePtr=newNodePtr;
  }
    return;
}

void insertLatLonEnd(double latIn, double lonIn)
{
  struct node *newNodePtr,*loopPtr;             // new node ptr and loop ptr
  newNodePtr=(struct node *)malloc(sizeof(struct node));
  newNodePtr->Lat=latIn;
  newNodePtr->Lon=lonIn;
  if(headNodePtr==NULL)    //if no list yet
  {
    newNodePtr->prevPtr=newNodePtr->nextPtr=NULL;
    headNodePtr=newNodePtr;
    tailNodePtr=newNodePtr;
    initNode(newNodePtr);
  }
  else
  {
    loopPtr=headNodePtr;
    while(loopPtr->nextPtr!=NULL)
    {
      loopPtr=loopPtr->nextPtr;
    }
   newNodePtr->prevPtr=loopPtr;
   loopPtr->nextPtr=newNodePtr;
   newNodePtr->nextPtr=NULL;
   tailNodePtr=newNodePtr;
   initNode(newNodePtr);
  }
  // uncomment to show Lat Lon after insertion in list
  //printf("function insertLatLonEnd()\n");
  //printf("newNodePtr->Lat: %lf\n", newNodePtr->Lat);
  //printf("newNodePtr->Lon: %lf\n\n", newNodePtr->Lon);


//displayFwd();
//printf("\n");

     return;
}

int initNode(struct node *currNodePtr)
{
  if(currNodePtr==NULL)
  {
    printf("No such node\n"); return 1;
  }
  //currNodePtr->Lat = 0.0;
  //currNodePtr->Lon = 0.0;
  currNodePtr->legMeters = 0.0;
  currNodePtr->heading = 0.0;
  currNodePtr->legSecs = 0.0;
  currNodePtr->wptNumber = 0;
  return 0;
}

int displayNode(struct node *currNodePtr)
{
  if(currNodePtr==NULL)  // no list exists
  {
    printf("No such node\n"); return 1;
  }
  else
  {
    printf("\n\nLat %.4f ", currNodePtr->Lat);
    printf("\nLon %.4f ", currNodePtr->Lon);
    printf("\nDistance: %.1f ", currNodePtr->legMeters);
    printf("\nHead %.1f ", currNodePtr->heading);
    printf("\nTime %.0f ", currNodePtr->legSecs);
    printf("\nwptNumber %d ", currNodePtr->wptNumber);
  }
  return 0;
}

void insertBeg(double legMetersIn)    // insert node at the beginning of DLL
{
  int fail = 0;
  struct node *newNodePtr;
  newNodePtr=(struct node *)malloc(sizeof(struct node));
  newNodePtr->legMeters = legMetersIn;
  if(headNodePtr==NULL)
  {
    newNodePtr->prevPtr=newNodePtr->nextPtr=NULL;
    headNodePtr=newNodePtr;
    tailNodePtr=newNodePtr;
  }
  else
  {
    newNodePtr->nextPtr=headNodePtr;
    newNodePtr->prevPtr=NULL;
    headNodePtr->prevPtr=newNodePtr;
    headNodePtr=newNodePtr;
  }
  fail=initNode(newNodePtr);
  if ( fail == 1 )
  {
    printf("\n\n***initNode fail in insertBeg()***\n\n");
    abort();
  }
  return;
}

void insertEnd(double legMetersIn)          // insert node at the End of DLL
{
  int fail = 0;
  struct node *newNodePtr,*loopPtr;             // new node ptr and loop ptr
  newNodePtr=(struct node *)malloc(sizeof(struct node));
  newNodePtr->legMeters = legMetersIn;
  if(headNodePtr==NULL)    //if no list yet
  {
    newNodePtr->prevPtr=newNodePtr->nextPtr=NULL;
    headNodePtr=newNodePtr;
    tailNodePtr=newNodePtr;
  }
  else
  {
    loopPtr=headNodePtr;
    while(loopPtr->nextPtr!=NULL)
    {
      loopPtr=loopPtr->nextPtr;
    }
    newNodePtr->prevPtr=loopPtr;
    loopPtr->nextPtr=newNodePtr;
    newNodePtr->nextPtr=NULL;
    tailNodePtr=newNodePtr;
  }
  fail=initNode(newNodePtr);
  if ( fail == 1 )
  {
    printf("\n\n***initNode fail in insertEnd()***\n\n");
    abort();
  }
  return;
}

void displayFwd(void)
{
  int fail=0;
  struct node *currNodePtr;
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return;
  }
  currNodePtr=headNodePtr;
  while(currNodePtr!=NULL)
  {
    fail=displayNode(currNodePtr);
    if ( fail == 1 )
    {
      printf("\n\n***displayNode fail in displayFwd()***\n\n");
    }
    currNodePtr=currNodePtr->nextPtr;
  }
  return;
}

void displayRev(void)
{
  int fail=0;
  struct node *currNodePtr;
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return;
  }
  currNodePtr=tailNodePtr;
  while(currNodePtr->prevPtr!=NULL)
  {
    fail=displayNode(currNodePtr);
    if ( fail == 1 )
    {
      printf("\n\n***displayNode fail in displayRev()***\n\n");
    }
    currNodePtr=currNodePtr->prevPtr;
  }
  displayNode(headNodePtr);
  return;
}

void insertNode(struct node *currNodePtr, double smallerlegMeters)
{
  struct node *newNodePtr;
  if(headNodePtr==NULL)  // no list exists
  {
    printf("dll is empty\n"); return;
  }
  newNodePtr=(struct node *)malloc(sizeof(struct node));

  newNodePtr->prevPtr=currNodePtr;
  newNodePtr->nextPtr=currNodePtr->nextPtr;
  newNodePtr->nextPtr->prevPtr=newNodePtr;
  currNodePtr->nextPtr=newNodePtr;

  // Initialize new node
  newNodePtr->Lat=0.0;
  newNodePtr->Lon=0.0;
  newNodePtr->legMeters=smallerlegMeters;
  newNodePtr->heading=newNodePtr->prevPtr->heading;
  newNodePtr->legSecs=0.0;
  newNodePtr->wptNumber=0;

  return;
}

void insertIfBig_legMeters(void)
{
//checks nodes if any legMeters is > maxLegMeters then insert a node before the long node
//  w adjusted legMeters in current node and new node
//  head node is special case, need to insert after it
  struct node *currNodePtr;
  double smallerlegMeters = 0;
//  int pass = 1;  // count number of times thru loop
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return;
  }
  do
  {
    big_legMeters = 0.0;    // biggest transit time found in the linked list
    currNodePtr=headNodePtr;  // head of list
    while(currNodePtr!=NULL)  // traverse list once
    {
//      printf("\n*** pass: %d ***", pass);
      if ( currNodePtr->legMeters > big_legMeters )
      {
        big_legMeters = currNodePtr->legMeters;
//        printf("\nbig_legMeters: %g", big_legMeters);
      }
      if ( currNodePtr->legMeters > maxLegMeters )
      {
        smallerlegMeters = currNodePtr->legMeters-maxLegMeters;


//        printf("\nBefore Insert, currNodePtr->legMeters: %g\n", currNodePtr->legMeters );
//        printf("smallerlegMeters:         %g\n", smallerlegMeters );

        currNodePtr->legMeters = maxLegMeters;
        insertNode( currNodePtr, smallerlegMeters );

//        printf("currNodePtr->heading: %g\n", currNodePtr->heading );
//        printf("After Insert, currNodePtr->legMeters: %g\n", currNodePtr->legMeters );
//        printf("smallerlegMeters:         %g\n", smallerlegMeters );

//       printf("\n\n********** Pass: %d **********", pass);
//        displayFwd();
//        pass++;

        break;

      }
      currNodePtr=currNodePtr->nextPtr;  // seek next node
    }
  } while ( big_legMeters > maxLegMeters );  // do while is done when all legMeterss < maxLegMeters
//  pass=1; // done with loop count
  return;
}

int fillBlankLatLons()
{
// fill in the blank Lat/Lon's
  struct node *currNodePtr;
    currNodePtr=headNodePtr;  // head of list
    while(currNodePtr!=NULL)  // traverse list once
        {
          if (currNodePtr->Lat == 0)  // found node which needs Lat Lon filled in
          {
          //printf("\n---call newLatLon...Display---\n");
          newLatLon(currNodePtr->prevPtr->Lat, currNodePtr->prevPtr->Lon, currNodePtr->prevPtr->heading, currNodePtr->prevPtr->legMeters, &currNodePtr->Lat, &currNodePtr->Lon);
          //displayNode(currNodePtr);
          }
          currNodePtr=currNodePtr->nextPtr;  // seek next node
        }
  return(0);
}

void itemizeList()
{
// traverse list, increment node member wptNumber
// starting with 1
  struct node *currNodePtr;
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return;
  }
  currNodePtr=headNodePtr;  // head of list
  int wpt_Number = 1;
  while(currNodePtr!=NULL)  // traverse list once
  {
    currNodePtr->wptNumber = wpt_Number++;
    currNodePtr=currNodePtr->nextPtr;  // seek next node
  }
  return;
}

/*
double calcMissionTime()
{
// traverse list, add up legMeters
//   divide sum by speed over ground in meters per second
//

  double legMetersSum = 0.0;
  double rad_diveAngle;
  double cos_diveAngle;
  struct node *currNodePtr;
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return (-1);
  }
  currNodePtr=headNodePtr;  // head of list
  while(currNodePtr!=NULL)  // traverse list once
  {
    legMetersSum = legMetersSum + currNodePtr->legMeters;
    currNodePtr=currNodePtr->nextPtr;  // seek next node
  }

  rad_diveAngle = diveAngle * DEGREES_TO_RADS;
  cos_diveAngle = cos(rad_diveAngle);

  // added 29 Sept 14  survey line total distance in meters
  surveyLineMeters = legMetersSum;

  missionTime = legMetersSum / ( missionSpeed * cos_diveAngle );

  // set to 120% of estimated time based only on leg transits
  missionTime = missionTime * 1.00; // was multiplied by 1.2, modified 27 Sept 14

  return (missionTime);
}
*/

// Calculate mission time using legSecs
//
// 3 Oct 2016 drthom
//
double calcMissionTime()
{
 // traverse list once, add up legSecs
 // return legSecsSum
 double legSecsSum = 0.0;
 double legMetersSum = 0.0;

 struct node *currNodePtr;
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return (-1);
  }
  currNodePtr=headNodePtr;  // head of list
  while(currNodePtr!=NULL)  // traverse list once
  {
    legSecsSum = legSecsSum + currNodePtr->legSecs;         // add current leg time to running time total
    legMetersSum = legMetersSum + currNodePtr->legMeters;   // add current leg distance to running distance total
    currNodePtr=currNodePtr->nextPtr;  // seek next node
  }

  surveyLineMeters = legMetersSum;

  return legSecsSum;
}



void calcWaypointTimes()
{
// traverse list, calculate each time to next waypoint
// except the final node
  struct node *currNodePtr;
  if(headNodePtr==NULL)
  {
    printf("dll is empty\n");
    return;
  }
  currNodePtr=headNodePtr;  // head of list
  while(currNodePtr!=NULL && currNodePtr!=tailNodePtr)  // traverse list once
  {
    currNodePtr->legSecs = timeToNextWayPoint( currNodePtr->legMeters );
    currNodePtr=currNodePtr->nextPtr;  // seek next node
  }
}

int launch_waypoint(void)
{
// insert new head node 500 meters from waypoint 1 on recip course
//
  double currentLat;
  double currentLon;
  double headingDeg;
  double headingDegPass;
  double distance;

  struct node *newNodePtr;
  newNodePtr=(struct node *)malloc(sizeof(struct node));
  newNodePtr->nextPtr=headNodePtr;
  newNodePtr->prevPtr=NULL;
  headNodePtr->prevPtr=newNodePtr;
  headNodePtr=newNodePtr;

  currentLat = headNodePtr->nextPtr->Lat;
  currentLon = headNodePtr->nextPtr->Lon;
  headingDeg = headNodePtr->nextPtr->heading;  // need to pass recip

  if (headingDeg > 180)
  {
    headingDegPass = headingDeg - 180;
  }
  else
  {
    headingDegPass = headingDeg + 180;
  }
  if (headingDeg == 180)
  {
    headingDegPass = 0;
  }

  distance = LAUNCH_OFFSET;
  newLatLon(currentLat, currentLon, headingDegPass, distance, &headNodePtr->Lat, &headNodePtr->Lon);

  headNodePtr->legMeters = distance;
  headNodePtr->heading = headingDeg;
  headNodePtr->legSecs = distance * 1.30;
  headNodePtr->wptNumber = 0;

//  displayNode(headNodePtr);

  return(0);
}

void destroyList(void)
{
  struct node *iteratePtr;
  struct node *currNodePtr;
  iteratePtr = headNodePtr;
  while (iteratePtr)
  {
    currNodePtr = iteratePtr;
    iteratePtr = currNodePtr->nextPtr;
    free(currNodePtr);
  }
  return;
}