/* -*-c++-*-
 * Delta3D Open Source Game and Simulation Engine
 * Copyright (C) 2006, Alion Science and Technology, BMH Operation
 *
 * This library is free software; you can redistribute it and/or modify it under
 * the terms of the GNU Lesser General Public License as published by the Free
 * Software Foundation; either version 2.1 of the License, or (at your option)
 * any later version.
 *
 * This library 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 Lesser General Public License for more
 * details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this library; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * William E. Johnson II
 */

#ifndef DELTA_FIRE_FIGHTER_UTILITY_FUNCTIONS
#define DELTA_FIRE_FIGHTER_UTILITY_FUNCTIONS

#include <dtCore/transform.h>
#include <osg/Math>

const float GRAVITY = 9.8f;                  //meters per second^2
const float yardsPerNM = 2000.0f;
const float secsPerHour = 3600.0f;
const float metersPerKnotPerSecond = 0.51283f;

enum Task { NONE, ATTACK_TARGET };

struct UFWaypoint
{
   float x;
   float y;
   float altitude;
   float speed;
   float desiredArrivalTime;
   Task task;

   UFWaypoint()
   {
      x = 0.0f;
      y = 0.0f;
      altitude = 0.0f;
      speed = 0.0f;
      desiredArrivalTime = 0.0f;
      task = NONE;
   }

   UFWaypoint(float tX, float tY, float tAltitude, float tSpeed,
            float tDesiredArrivalTime = 0.0f, Task tTask = NONE)
   {
      x = tX;
      y = tY;
      altitude = tAltitude;
      speed = tSpeed;
      desiredArrivalTime = tDesiredArrivalTime;
      task = tTask;
   }
};

inline void AdjustX(dtCore::Transform* startPos, float newX, bool relative = false)
{
   float x1, y1, z1, h1, p1, r1;

   startPos->Get(x1, y1, z1, h1, p1, r1);

   if (relative)
   {
      newX += x1;
   }

   startPos->Set(newX, y1, z1, h1, p1, r1);
}

inline void AdjustY(dtCore::Transform* startPos, float newY, bool relative = false)
{
   float x1, y1, z1, h1, p1, r1;

   startPos->Get(x1, y1, z1, h1, p1, r1);

   if (relative)
   {
      newY += y1;
   }

   startPos->Set(x1, newY, z1, h1, p1, r1);
}

inline void AdjustZ(dtCore::Transform* startPos, float newZ, bool relative = false)
{
   float x1, y1, z1, h1, p1, r1;

   startPos->Get(x1, y1, z1, h1, p1, r1);

   if (relative)
   {
      newZ += z1;
   }

   startPos->Set(x1, y1, newZ, h1, p1, r1);
}

inline void AdjustH(dtCore::Transform* startPos, float newH, bool relative = false)
{
   float x1, y1, z1, h1, p1, r1;

   startPos->Get(x1, y1, z1, h1, p1, r1);

   if (relative)
   {
      newH += h1;
   }

   startPos->Set(x1, y1, z1, newH, p1, r1);
}

inline void AdjustP(dtCore::Transform* startPos, float newP, bool relative = false)
{
   float x1, y1, z1, h1, p1, r1;

   startPos->Get(x1, y1, z1, h1, p1, r1);

   if (relative)
   {
      newP += p1;
   }

   startPos->Set(x1, y1, z1, h1, newP, r1);
}

inline void AdjustR(dtCore::Transform* startPos, float newR, bool relative = false)
{
   float x1, y1, z1, h1, p1, r1;

   startPos->Get(x1, y1, z1, h1, p1, r1);

   if (relative)
   {
      newR += r1;
   }

   startPos->Set(x1, y1, z1, h1, p1, newR);
}

inline float ComputeDistance(float x1, float y1, float x2, float y2)
{
   return sqrtf(osg::square(x2 - x1) + osg::square(y2 - y1));
}

/*
 *  Bearing from x1,y1 to x2,y2...
 */
inline float ComputeBearingTo(float x1, float y1, float x2, float y2)
{
   return 180.0f - osg::RadiansToDegrees(atan2f(x2 - x1, y2 - y1));
}

inline void ComputeNewCoordinates(float x1, float y1, float distance, float angle, float &x2, float &y2)
{
   x2 = x1 + distance * sinf(osg::DegreesToRadians(angle));
   y2 = y1 - distance * cosf(osg::DegreesToRadians(angle));
}

inline float ConvertTodtCoreBearing(float bearing)
{
   return 360.0f - bearing;
}

inline float ConvertFromdtCoreBearing(float bearing)
{
   return 360.0f - bearing;
}

inline void VerifyBearingWithinBounds(float& bearing)
{
   while (bearing >= 360.0f)
   {
      bearing -= 360.0f;
   }

   while (bearing < .0f)
   {
      bearing += 360.0f;
   }
}

inline dtCore::Transform Offset2DPosition(dtCore::Transform* startPos, dtCore::Transform* offsetPos)
{
   dtCore::Transform tempPos;
   float x1, y1, z1, h1, p1, r1;
   float x2, y2, z2, h2, p2, r2;

   startPos->Get(x1, y1, z1, h1, p1, r1);
   offsetPos->Get(x2, y2, z2, h2, p2, r2);

   float newX2 = x2 * cosf(osg::DegreesToRadians(h1)) - y2 * sinf(osg::DegreesToRadians(h1));
   float newY2 = x2 * sinf(osg::DegreesToRadians(h1)) + y2 * cosf(osg::DegreesToRadians(h1));

   tempPos.Set(x1 + newX2, y1 + newY2, z2, h1 + h2, p2, r2);

   return tempPos;
}

inline dtCore::Transform Difference2DPosition(dtCore::Transform* startPos, dtCore::Transform* offsetPos)
{
   dtCore::Transform tempPos;
   float x1, y1, z1, h1, p1, r1;
   float x2, y2, z2, h2, p2, r2;

   startPos->Get(x1, y1, z1, h1, p1, r1);
   offsetPos->Get(x2, y2, z2, h2, p2, r2);

   float bearing = 0.0f - h1;

   float newX2 = (x2 - x1) * cosf(osg::DegreesToRadians(bearing)) - (y2 - y1) * sinf(osg::DegreesToRadians(bearing));
   float newY2 = (x2 - x1) * sinf(osg::DegreesToRadians(bearing)) + (y2 - y1) * cosf(osg::DegreesToRadians(bearing));

   tempPos.Set(newX2, newY2, z2, h2 - h1, p2, r2);
   return tempPos;
}

#endif