#include <sys/stat.h>
#include <dirent.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <netinet/in.h>
#include <sys/time.h>

#include "Syslog.h"
#include "Attributes.h"
#include "AttributeParser.h"
#include "BooleanAttribute.h"
#include "FloatAttribute.h"
#include "IntegerAttribute.h"
#include "StringAttribute.h"
#include "TerrainNavClient.h"

Attributes _attributes("../config/docking/terrainAid.cfg");

char *_config;
char  _mapFile[128];
char *_mapFileName;
char *_particlesName;
char  _vehicleCfg[128];
char *_vehicleCfgName;
char  _dvlCfg[128];
char *_dvlCfgName;
char  _resonCfg[128];
char *_resonCfgName;
char *_terrainNavServer;
long  _terrainNavPort;
long  _map_type;

long _samplePeriod;
Boolean _forceLowGradeFilter;
Boolean _allowFilterReinits;
Boolean _useModifiedWeighting;
Boolean _notUsed;
double _depth;
double _navTime0, _dvlTime0;
int _numReinits;
int _filterState;
int _reinitFilter;

double _maxNorthingCov;
double _maxEastingCov;
double _maxNorthingError;
double _maxEastingError;
double _phiBias;
Boolean _useIDTData;
Boolean _useDvlSide;

void createCfgAttributes()
{
   char nameStr[64];

//
//  Don't forget that StringAttribute allocates the memory itself, so there
//  is no need to allocate it here.
//
   long NotSpecified = -1;
   sprintf( nameStr, "mapFileName" );
   _attributes.add( new StringAttribute(nameStr, nameStr, 
          &_mapFileName, "NotSpecified" ) );
   sprintf( nameStr, "map_type" );
   _attributes.add( new IntegerAttribute(nameStr, nameStr, 
          &_map_type, 1 ) );

   sprintf( nameStr, "particlesName" );
   _attributes.add( new StringAttribute(nameStr, nameStr, 
          &_particlesName, "NotSpecified" ) );

   sprintf( nameStr, "vehicleCfgName" );
   _attributes.add( new StringAttribute(nameStr, nameStr, 
          &_vehicleCfgName, "NotSpecified" ) );

   sprintf( nameStr, "dvlCfgName" );
   _attributes.add( new StringAttribute(nameStr, nameStr, 
          &_dvlCfgName, "NotSpecified" ) );

   sprintf( nameStr, "resonCfgName" );
   _attributes.add( new StringAttribute(nameStr, nameStr, 
          &_resonCfgName, "NotSpecified" ) );

   sprintf( nameStr, "terrainNavServer" );
   _attributes.add( new StringAttribute(nameStr, nameStr, 
          &_terrainNavServer, "NotSpecified" ) );

   sprintf( nameStr, "terrainNavPort" );
   _attributes.add( new IntegerAttribute(nameStr, nameStr, 
          &_terrainNavPort, 27027 ) );

   sprintf( nameStr, "forceLowGradeFilter" );
   _attributes.add( new BooleanAttribute(nameStr, nameStr, 
           &_forceLowGradeFilter, NotSpecified ) );

   sprintf( nameStr, "allowFilterReinits" );
   _attributes.add( new BooleanAttribute(nameStr, nameStr, 
           &_allowFilterReinits, NotSpecified ) );

   sprintf( nameStr, "useModifiedWeighting" );
   _attributes.add( new BooleanAttribute(nameStr, nameStr, 
           &_useModifiedWeighting, NotSpecified ) );

   sprintf( nameStr, "samplePeriod" );
   _attributes.add( new IntegerAttribute(nameStr, nameStr, 
           &_samplePeriod, NotSpecified ) );

   _attributes.add( new FloatAttribute("maxNorthingCov",
                   "Maximum northing covariance", &_maxNorthingCov, 
              NotSpecified ) );

   _attributes.add( new FloatAttribute("maxEastingCov",
                   "Maximum easting covariance", &_maxEastingCov, 
              NotSpecified ) );

   _attributes.add( new FloatAttribute("maxNorthingError",
                   "Maximum northing error", &_maxNorthingError, 
              NotSpecified ) );

   _attributes.add( new FloatAttribute("maxEastingError",
                   "Maximum easting error", &_maxEastingError, 
              NotSpecified ) );

   _attributes.add( new FloatAttribute("RollOffset",
                   "Roll Offset", &_phiBias, 
              NotSpecified ) );

   sprintf( nameStr, "useIDTData" );
   _attributes.add( new BooleanAttribute(nameStr, nameStr, 
          &_useIDTData, False ) );

   sprintf( nameStr, "useDVLSide" );
   _attributes.add( new BooleanAttribute(nameStr, nameStr, 
          &_useDvlSide, False ) );

   AttributeParser::parse("../config/docking/terrainAid.cfg", &_attributes); 

}

char latestLogDirName(char filename[], size_t namelen)
{
  struct stat latest;
  struct stat log;
  struct dirent *dp;
  char *logs = getenv("AUV_LOG_DIR");
  if (!logs)
  {
    Syslog::write("System::latestLogDirName() - AUV_LOG_DIR undefined");
    return 0;
  }

  // Retrieve the stat info of the directory that latest points to
  // 
  char latestpath[300];
  sprintf(latestpath, "%s/latest", logs);
  if (access(latestpath, F_OK) < 0)
  {
    Syslog::write("System::latestLogDirName() - Could not find %s", latestpath);
    return 0;
  }

  stat(latestpath, &latest);

  // Open the logs directory
  // 
  DIR *dirp = opendir(logs);
  if (!dirp)
  {
    Syslog::write("System::latestLogDirName() - Could not open %s", logs);
    return 0;
  }

  // Look for matching inodes
  //
  while (dp = readdir(dirp))
  {
    if (dp->d_stat.st_ino == latest.st_ino)
    {
      if (strcmp(dp->d_name, "latest"))    // Don't really have to do this, but...
      {
        filename[0] = '\0';
        sprintf(filename, "%s-TRN/", basename(dp->d_name));
        return 1;                          // Found it!
      }
    }
  }

  Syslog::write("System::latestLogDirName() - Could not find what %s points to", latestpath);
  return 0;
}

int main(int argc, char* argv[])
{
  // Get the connection paramteres from the command line
  //
  char *host="", *map="", *cfg="", *lof=0, *par="";
  long port=27027;
  int  mtype=0;
  int c;
  while ( (c = getopt(argc, argv, "h:p:m:t:c:l:r:")) != EOF )
  {
    if (c == 'h')
      host = strdup(optarg);
    else if (c == 'm')
      map = strdup(optarg);
    else if (c == 't')
      mtype = atoi(optarg);
    else if (c == 'c')
      cfg = strdup(optarg);
    else if (c == 'l')
      lof = strdup(optarg);
    else if (c == 'r')
      par = strdup(optarg);
    else if (c == 'p')
      port = atol(optarg);
  }

  // Gotta have everything but the port number to connect
  //
  if (!host || !map || !cfg || (mtype != 1 && mtype != 2) )
  {
    printf("usage:\n test_client  -h host.ip  -m map.bo  -t maptype {1=DEM|2=Oct}  -c auv_specs.cfg [-r particles] [-p port] [-l logdir]\n");
    printf("example:\n");
    printf(" test_client  -h 134.89.32.110  -m Topo_Wall.bo  -t 2  -c mappinAUV_specs.cfg -r particles.cfg\n");
    return -1;
  }

  char logdir[256];
  if (!lof)
    latestLogDirName(logdir, sizeof(logdir));
  else
    sprintf(logdir, "%s/", lof);

  TerrainNavClient *_tercom;

  // Server will use TRN_MAPFILES environment variable on the server
  // for location of map files, so just use the filename here.
  //
  Syslog::write("TerrainAidDriver: Using TerrainNavClient at %s on port %d",
   host, port);
  Syslog::write("TerrainAidDriver: Using TerrainNavClient with map %s and config %s",
   map, cfg);

  _tercom = new TerrainNavClient(host, port, map, cfg, par, logdir, 2, mtype);

  // After moving is_connected() to public section, we can use this code block
  //
  if (!_tercom->is_connected())
  {
    Syslog::write("TerrainAidDriver::Not connected. See trn_server error messages...");
    return -1;
  }

  // If we reach here then we've connected
  //
  Syslog::write("TerrainAidDriver::Should be Connected to server if no error messages...");

  Syslog::write("TerrainAidDriver::Lets just set the interpret measure attitude flag to true...");
  sleep(2);
  _tercom->setInterpMeasAttitude(true);

  int fs = _tercom->getFilterState();
  Syslog::write("TerrainAid test client - filter state is %d", fs);

  for (int i = 0; i < 5; i++)
  {
    try
    {
      if (!_tercom->is_connected())
        Syslog::write("TerrainAid test client - server not connected!");

      Syslog::write("TerrainAid test client - # reinits is %d", _tercom->getNumReinits());
    }
    catch (Exception e)
    {
      Syslog::write("Caught exception %s", e.msg);
    }

    sleep(3);
  }

  Syslog::write("Done. Close the connection...");

  return 0;
}