//------------------------------------------------------------------------------
// test2.cpp - run a single track through a Hermite interpolation

#include <iostream>
#include <fstream>
#include "SpatialDataManager.h"
#include "EarthModel.h"

using std::cout;
using std::endl;
using std::ofstream;

const double ERADIUS = 6378108.52;

void gather(SpatialDataManager* sdm, SpatialID trkID)
{
   // Open capture file
   ofstream ofs("hermite_output.idf");
   
   if (ofs)
   {
      double xECEF, yECEF, zECEF;
      EarthModel::ConvertENUToECEF(0.0, 0.0, 0.0,   // lat / lon, alt
                                   0.0, 0.0, 0.0,   // xENU, yENU, zENU
                                   true,            // positionData
                                   xECEF, yECEF, zECEF);
               
      ofs << "PLATFORM^OWNSHIP^0.0^1^1^1.4^"
          << "SEA_SURFACE^FRIEND^NONE^"
          << xECEF << "^"
          << yECEF << "^"
          << zECEF << "^"
          << "0.0^0.0^0.0^0^\n";
          
      SpatialData  spatialData;
      
      for(double time=0.0; time<900.0; time+= 5.0)
      {
         spatialData.updateTime = time;        // set time of request
         
         sdm->GetEntity(trkID, time, spatialData);   // get the data
         
         ofs << "PLATFORM^TRACK^" << time 
             << "2^2^2^1.2^"
             << "AIR^HOSTILE^NONE^"
             << spatialData.pX << "^"
             << spatialData.pY << "^"
             << spatialData.pZ << "^"
             << spatialData.vX << "^"
             << spatialData.vY << "^"
             << spatialData.vZ << "^"
             << "0\n";
      }
   
      // Close capture file
      ofs.close();
   }
   else
   {
      cout << "Crap! couldn't open output file" << endl;
   }
   

}


int main()
{


   SpatialID   trkID = NULL_SPATIALID;

   SpatialData my_trk_sd;
   SpatialData first_point;
   SpatialData second_point;
      
   my_trk_sd.pX    =  ERADIUS;
   my_trk_sd.pY    = 128000.0;
   my_trk_sd.pZ    =   1000.0;
   
   my_trk_sd.vX = 0.0;
   my_trk_sd.vY = 0.0;
   my_trk_sd.vZ = 0.0;

   my_trk_sd.aX = 0.0;
   my_trk_sd.aY = 0.0;
   my_trk_sd.aZ = 0.0;
   
   // Hermite Points
   
   first_point.updateTime  = 0.0;
   
   first_point.pX   =  ERADIUS;
   first_point.pY   = 100000.0;
   first_point.pZ   =   1000.0;
   
   first_point.vX   =  90.0;
   first_point.vY   = 100.0;
   first_point.vZ   =   0.0;
   
   first_point.aX   = 0.0;
   first_point.aY   = 0.0;
   first_point.aZ   = 0.0;

   second_point.updateTime = 900.0;         
   
   second_point.pX  =   ERADIUS;
   second_point.pY  =   10.0;
   second_point.pZ  =  200.0;
   
   second_point.vX  =  -90.0;
   second_point.vY  =  600.0;
   second_point.vZ  = -300.0;

   second_point.aX  =  0.0;
   second_point.aY  =  0.0;
   second_point.aZ  =  0.0;



   // Get Pointer to Spatial Datamanager Instance
   SpatialDataManager* sdm = SpatialDataManager::Instance();

   trkID = sdm->RegisterEntity(DRM::ORDER_THREE, my_trk_sd);
                   
   // At this point we should have a valid SpatialID in hand
   if (trkID != NULL_SPATIALID)
   {
      // Okay, lets set the Hermite parameters
      sdm->SetHermite(trkID, first_point, second_point);
      
      // Lets gather data for this thing over the 90 second interval
      gather(sdm, trkID);
      
   }
   else
   {
      cout << "Crap! Registration callback mechanism failure?!" << endl;
   }
   
   return 0;
}