/* File: structDefs.cpp
 * -------------------
 * Written by: Debbie Meduna
 *****************************************************************************/

#include "structDefs.h"

/*----------------------------------------------------------------------------
/mapT member functions
/----------------------------------------------------------------------------*/
mapT::mapT()
{
   xpts = NULL;
   ypts = NULL;
   numX = 0;
   numY = 0;
}

mapT::~mapT()
{
   clean();
}

void mapT::clean()
{
   if(xpts != NULL)
   {
      delete [] xpts;
      xpts = NULL;
   }
  
   if(ypts != NULL)
   {
      delete [] ypts;
      ypts = NULL;
   }
  
   depths.CleanUp();
   depthVariance.CleanUp();
}

void mapT::reSampleMap(const double newRes)
{
   int newNumX, newNumY;
   double* xptsNew;
   double* yptsNew;
   Matrix depthsNew;
   Matrix depthVarNew;
   int i, j, subRes;
   Matrix subDepthMap;

   //Fill in new xpts/ypts vectors
   newNumX = int(round(fabs(xpts[numX-1]-xpts[0])/newRes))+1;
   newNumY = int(round(fabs(ypts[numY-1]-ypts[0])/newRes))+1;
   depthsNew.ReSize(newNumX, newNumY);
   depthVariance.ReSize(newNumX, newNumY);

   xptsNew = new double[newNumX];
   yptsNew = new double[newNumY];

   for(i = 0; i < newNumX; i++)
      xptsNew[i] = xpts[0]+newRes*i;
  
   for(j = 0; j < newNumY; j++)
      yptsNew[j] = ypts[0]+newRes*j;

   //Fill in new depth values
   if((newRes > dx) | (newRes > dy))
   {
      //TO DO: Fix this so it actually computes correct average for newRes!!
      subRes = int(ceil(newRes/dx));
      subDepthMap.ReSize(subRes, subRes);
      for(i = 1; i <= newNumX; i++)
      {
         for(j = 1; j <= newNumY; j++)
         {
            //fill in new depth values by averaging subMatrix
            subDepthMap = depths.SubMatrix((i-1)*subRes+1,i*subRes,
                                           (j-1)*subRes+1,j*subRes);
            depthsNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();

            //fill in new depth variance values
            subDepthMap = depthVariance.SubMatrix((i-1)*subRes+1,i*subRes,
                                                  (j-1)*subRes+1,j*subRes);
            depthVarNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();
         }
      }
   }
   else
      interp2mat(xpts,ypts,depths,xptsNew,yptsNew,depthsNew);

   //Remove old map and assign new values;
   clean();
   dx = newRes;
   dy = newRes;
   numX = newNumX;
   numY = newNumY;
   xpts = xptsNew;
   ypts = yptsNew;
   depths = depthsNew;
   depthVariance = depthVarNew;
   xcen = (xpts[numX-1] + xpts[0])/2.0;
   ycen = (ypts[numY-1] + ypts[0])/2.0;
}

//subSample the stored map to a lower resolution.
void mapT::subSampleMap(const int subRes)
{
   double newResX, newResY;
   int newNumX, newNumY, count,i, j;
   double* xptsNew;
   double* yptsNew;
   Matrix depthsNew;
   Matrix depthVarNew;
   Matrix subDepthMap(subRes, subRes);

   //Fill in new xpts/ypts vectors
   newNumX = int(numX/subRes);
   newNumY = int(numY/subRes);
   depthsNew.ReSize(newNumX, newNumY);
   depthVarNew.ReSize(newNumX, newNumY);

   xptsNew = new double[newNumX];
   yptsNew = new double[newNumY];

   count = 0;
   for(i = 0; i < numX && count < newNumX; i=i+subRes)
   {
      xptsNew[count] = xpts[i];
      count++;
   }
  
   count = 0;
   for(j = 0; j < numY && count < newNumY; j=j+subRes)
   {
      yptsNew[count] = ypts[j];
      count++;
   }

   //Fill in new depths matrix by averaging depths in the cells;
   for(i = 1; i <= newNumX; i++)
   {
      for(j = 1; j <= newNumY; j++)
      {
         //fill in new depth values by averaging subMatrix
         subDepthMap = depths.SubMatrix((i-1)*subRes+1,i*subRes,
                                        (j-1)*subRes+1,j*subRes);
         depthsNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();

         //fill in new depth variance values
         subDepthMap = depths.SubMatrix((i-1)*subRes+1,i*subRes,
                                        (j-1)*subRes+1,j*subRes) 
            - depthsNew(i,j);
         subDepthMap = SP(subDepthMap, subDepthMap);
         depthVarNew(i,j) = double((1.0/(subRes*subRes)))*subDepthMap.Sum();
      }
   }

   newResX = dx*subRes;
   newResY = dy*subRes;

   //Remove old map and assign new values;
   clean();
   dx = newResX;
   dy = newResY;
   numX = newNumX;
   numY = newNumY;
   xpts = xptsNew;
   ypts = yptsNew;
   depths = depthsNew;
   depthVariance = depthVarNew;
   xcen = (xpts[numX-1] + xpts[0])/2.0;
   ycen = (ypts[numY-1] + ypts[0])/2.0;
}

//display map values in a more readable format
void mapT::displayMap()
{
   int i;

   //print a blank space in upper left corner
   output("%5s","");
   output("y:");

   //display ypt values
   for(i = 0; i < numY; i++)
      output("%5.2f", ypts[i]);
   output("\n x: \n");

   //display xpt values and depth values
   for(i = 0; i < numX; i++)
   {
      output("%5.2f", xpts[i]);
      output("%2s","");
      for(int j = 0; j < numY; j++)
         output("%5.2f", depths(i+1,j+1));
      
      output("\n");
   }
}

//copy assignment operator
mapT& mapT::operator=(mapT& rhs)
{
   if(this != &rhs)
   {
      this->clean();
      
      //copy non-array values
      dx = rhs.dx;
      dy = rhs.dy;
      xcen = rhs.xcen;
      ycen = rhs.ycen;
      numX = rhs.numX;
      numY = rhs.numY;
      depths = rhs.depths;
      depthVariance = rhs.depthVariance;
      
      //copy array values
      xpts = new double[numX];
      ypts = new double[numY];
      for(int i = 0; i < numX; i++)
         xpts[i] = rhs.xpts[i];
      for(int j = 0; j < numY; j++)
         ypts[j] = rhs.ypts[j];

   }
   return(*this);
}


/*----------------------------------------------------------------------------
/poseT member functions
/----------------------------------------------------------------------------*/
poseT::poseT()
{
   int i;

   //initialize values to zero
   x = 0.0;
   y = 0.0;
   z = 0.0;
   vx = 0.0;
   vy = 0.0;
   vz = 0.0;
   vw_x = 0;
   vw_y = 0;
   vw_z = 0;
   ax = 0.0;
   ay = 0.0;
   az = 0.0;
   phi = 0.0;
   theta = 0.0;
   psi = 0.0;
   wx = 0.0;
   wy = 0.0;
   wz = 0.0;
   time = 0.0;
   dvlValid = false;
   gpsValid = false;
   bottomLock = false;
   for(i = 0; i < 36; i++)
      covariance[i] = 0.0;
}


//copy assignment operator
poseT& poseT::operator=(poseT& rhs)
{
   if(this != &rhs)
   {
      //copy non-array values
      x = rhs.x;
      y = rhs.y;
      z = rhs.z; 
      vx = rhs.vx;
      vy = rhs.vy;
      vz = rhs.vz;
      vw_x = rhs.vw_x;
      vw_y = rhs.vw_y;
      vw_z = rhs.vw_z;
      ax = rhs.ax;
      ay = rhs.ay;
      az = rhs.az;
      phi = rhs.phi;
      theta = rhs.theta;
      psi = rhs.psi;
      wx = rhs.wx;
      wy = rhs.wy;
      wz = rhs.wz;
      time = rhs.time;
      dvlValid = rhs.dvlValid;
      gpsValid = rhs.gpsValid;
      bottomLock = rhs.bottomLock;
      //copy array values
      for (int i = 0; i < 36; i++)
         covariance[i] = rhs.covariance[i];
   }
   return(*this);
}

//difference assignment operator
poseT& poseT::operator-=(poseT& rhs)
{
   x -= rhs.x;
   y -= rhs.y;
   z -= rhs.z; 
   vx -= rhs.vx;
   vy -= rhs.vy;
   vz -= rhs.vz;
   vw_x -= rhs.vw_x;
   vw_y -= rhs.vw_y;
   vw_z -= rhs.vw_z;
   ax -= rhs.ax;
   ax -= rhs.ax;
   ay -= rhs.ay;
   az -= rhs.az;
   phi -= rhs.phi;
   theta -= rhs.theta;
   psi -= rhs.psi;
   wx -= rhs.wx;
   wy -= rhs.wy;
   wz -= rhs.wz;
   time -= rhs.time;
   dvlValid = (dvlValid && rhs.dvlValid);
   gpsValid = (gpsValid && rhs.gpsValid);
   bottomLock = (bottomLock && rhs.bottomLock);
   return(*this);
}

//addition assignment operator
poseT& poseT::operator+=(poseT& rhs)
{
   x += rhs.x;
   y += rhs.y;
   z += rhs.z; 
   vx += rhs.vx;
   vy += rhs.vy;
   vz += rhs.vz;
   vw_x += rhs.vw_x;
   vw_y += rhs.vw_y;
   vw_z += rhs.vw_z;
   ax += rhs.ax;
   ax += rhs.ax;
   ay += rhs.ay;
   az += rhs.az;
   phi += rhs.phi;
   theta += rhs.theta;
   psi += rhs.psi;
   wx += rhs.wx;
   wy += rhs.wy;
   wz += rhs.wz;
   time += rhs.time;
   dvlValid = (dvlValid && rhs.dvlValid);
   gpsValid = (gpsValid && rhs.gpsValid);
   bottomLock = (bottomLock && rhs.bottomLock);
   return(*this);
}
 
/*----------------------------------------------------------------------------
/measT member functions
/----------------------------------------------------------------------------*/
measT::measT()
{
   covariance = NULL;
   ranges = NULL;
   crossTrack = NULL;
   alongTrack = NULL;
   altitudes = NULL;
   measStatus = NULL;
   numMeas = 0; 
}

measT::~measT()
{
   clean();
}

//clean all dynamic memory elements of the struct
void measT::clean()
{
   if(covariance != NULL)
      delete [] covariance;
   covariance = NULL;
  
   if(ranges != NULL)
      delete [] ranges;
   ranges = NULL;
  
   if(crossTrack != NULL)
      delete [] crossTrack;
   crossTrack = NULL;
  
   if(alongTrack != NULL)
      delete [] alongTrack;
   alongTrack = NULL;
  
   if(altitudes != NULL)
      delete [] altitudes;
   altitudes = NULL;
  
   if(measStatus != NULL)
      delete [] measStatus;
   measStatus = NULL; 
}

//copy assignment operator
measT& measT::operator=(measT& rhs)
{
   int i;
   if(this != &rhs)
   {
      //if the two measT structs have different datatype or number of
      //measurements, we need to delete and recreate memory for the 
      //new measT struct.
      if(numMeas != rhs.numMeas || dataType != rhs.dataType)
      {
         this->clean();
         if(rhs.dataType == 2 || rhs.dataType == 4)
         {  
            crossTrack = new double[rhs.numMeas];
            alongTrack = new double[rhs.numMeas];
            altitudes = new double[rhs.numMeas];
         }
         else
            ranges = new double[rhs.numMeas];

         measStatus = new bool[rhs.numMeas];
         if(rhs.covariance != NULL)
            covariance = new double[rhs.numMeas];           
      }
      
      //copy non-array values
      time = rhs.time;
      dataType = rhs.dataType;
      phi = rhs.phi;
      theta = rhs.theta;
      psi = rhs.psi;
      numMeas = rhs.numMeas;
      x = rhs.x;
      y = rhs.y;
      z = rhs.z;
      
      //copy array values
      for (i = 0; i < rhs.numMeas; i++)
      {
         if(rhs.dataType == 2 || rhs.dataType == 4)
         {
	    crossTrack[i] = rhs.crossTrack[i];
            alongTrack[i] = rhs.alongTrack[i];
            altitudes[i] = rhs.altitudes[i];
         }
         else
            ranges[i] = rhs.ranges[i];

         measStatus[i] = rhs.measStatus[i];
         if(rhs.covariance != NULL)
            covariance[i] = rhs.covariance[i];
      }

   }
   return(*this);
}

/*----------------------------------------------------------------------------
/transformT member functions
/----------------------------------------------------------------------------*/
void transformT::displayTransformInfo()
{
   output("Rotation angles (phi, theta, psi): \n (%f ,%f, %f)\n", 
          rotation[0]*180/PI, rotation[1]*180/PI, rotation[2]*180/PI);
   output("Translation vector [dx, dy, dz]: \n (%f ,%f, %f)\n", dr[0], dr[1],
          dr[2]);
}

/*----------------------------------------------------------------------------
/sensorT member functions
/----------------------------------------------------------------------------*/
sensorT::sensorT()
{
   T_bs = NULL;
}

sensorT::sensorT(char* fileName)
{
   T_bs = NULL;
}

sensorT::~sensorT()
{
   if(T_bs != NULL)
      delete [] T_bs;
   T_bs = NULL;
}

void sensorT::parseSensorSpecs(char* fileName)
{
   fstream sensorFile;
   char temp[512];
   int i;
  
   sensorFile.open(fileName);
   if(sensorFile.is_open())
   {
      //read in sensor name
      sensorFile.ignore(256,':');
      sensorFile.getline(name,256);

      //read in sensor type
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      type = atoi(temp);

      //read in number of beams
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      numBeams = atoi(temp);

      //read in percent range error
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      percentRangeError = atof(temp);

      //read in beam width
      sensorFile.ignore(256,':');
      sensorFile.getline(temp,256);
      beamWidth = atof(temp)*PI/180.0;

      //read in beam information
      T_bs = new transformT[numBeams];
        
      if(type == 2)
      {
         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         T_bs[0].rotation[1] = atof(temp)*PI/180.0;

         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         double dphi = atof(temp)*PI/180.0;

         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         T_bs[0].rotation[2] = atof(temp)*PI/180.0;

         sensorFile.ignore(256,':');
         sensorFile.getline(temp,256);
         double dpsi = atof(temp)*PI/180.0;

         for(i = 0; i < numBeams; i++)
         {
            T_bs[i].rotation[1] = T_bs[0].rotation[1] + i*dphi;
            T_bs[i].rotation[2] = T_bs[0].rotation[2] + i*dpsi;
            T_bs[i].rotation[0] = 0.0;
            T_bs[i].dr[0] = 0.0;
            T_bs[i].dr[1] = 0.0;
            T_bs[i].dr[2] = 0.0;
         }
      }
      else
      {
         //beam pitch angle
         sensorFile.ignore(256,':');
         for(i = 0; i < numBeams; i++)
         {     
            if(i < numBeams-1)
               sensorFile.getline(temp,10,',');
            else
               sensorFile.getline(temp,10);
            T_bs[i].rotation[1] = atof(temp)*PI/180.0;
            T_bs[i].rotation[0] = 0.0;
            T_bs[i].dr[0] = 0.0;
            T_bs[i].dr[1] = 0.0;
            T_bs[i].dr[2] = 0.0;
         }

         //beam yaw angle
         sensorFile.ignore(256,':');
         for(i = 0; i < numBeams; i++)
         {     
            if(i < numBeams-1)
               sensorFile.getline(temp,10,',');
            else
               sensorFile.getline(temp,10);
            T_bs[i].rotation[2] = atof(temp)*PI/180.0;
         }
      }

      sensorFile.close();
   }
   else
   {
      printf("Error opening file %s.  Exiting...\n", fileName);
      exit(0);
   }

   return;
}

void sensorT::displaySensorInfo()
{
   output("Sensor name: %s\n", name);
   output("Sensor type: %i\n", type);
   output("Number of beams per measurement: %i\n", numBeams);
}


/*----------------------------------------------------------------------------
/vehicleT member functions
/----------------------------------------------------------------------------*/
vehicleT::vehicleT()
{
   T_sv = NULL;
   sensors = NULL;
}

vehicleT::vehicleT(char* fileName)
{
   T_sv = NULL;
   sensors = NULL;
   parseVehicleSpecs(fileName);
}

vehicleT::~vehicleT()
{
   if(T_sv != NULL)
      delete [] T_sv;
   T_sv = NULL; 

   if(sensors != NULL)
      delete [] sensors;
   sensors = NULL;
}

void vehicleT::parseVehicleSpecs(char* fileName)
{
   fstream vehicleFile;
   char temp[512];
   char temp2[512];
   char sensorFile[1024];
   char* sensorPath;
   
   vehicleFile.open(fileName);
   if(vehicleFile.is_open())
   {
      //read in vehicleName
      vehicleFile.ignore(256,':');
      vehicleFile.getline(name,256);

      //read in number of sensors
      vehicleFile.ignore(256,':');
      vehicleFile.getline(temp,256);
      numSensors = atoi(temp);      

      //read in INS drift rate
      vehicleFile.ignore(256,':');
      vehicleFile.getline(temp,256);
      driftRate = atof(temp);

      //read in sensor information
      sensors = new sensorT[numSensors];
      T_sv = new transformT[numSensors];

      for(int i = 0; i < numSensors; i++)
      {
         //sensor name
         vehicleFile.ignore(256,':');
         vehicleFile.getline(sensors[i].name,256);

         //sensor orientation offset
         vehicleFile.ignore(256,':');
         vehicleFile.getline(temp,10,',');
         T_sv[i].rotation[0] = atof(temp)*PI/180.0;
         vehicleFile.getline(temp,10,',');
         T_sv[i].rotation[1] = atof(temp)*PI/180.0;
         vehicleFile.getline(temp,10);
         T_sv[i].rotation[2] = atof(temp)*PI/180.0;

         //sensor translational offset
         vehicleFile.ignore(256,':');
         vehicleFile.getline(temp,10,',');
         T_sv[i].dr[0] = atof(temp);
         vehicleFile.getline(temp,10,',');
         T_sv[i].dr[1] = atof(temp);
         vehicleFile.getline(temp,10);
         T_sv[i].dr[2] = atof(temp);
      
         //extract file directory
         strcpy(sensorFile, fileName);
         sensorPath = strstr(sensorFile,name);
         
         //determine sensor file name
         sprintf(temp2, "%s%s",sensors[i].name,"_specs.cfg\0");
         strcpy(sensorPath,temp2);

         //parse sensor file 
         sensors[i].parseSensorSpecs(sensorFile);
      }

      vehicleFile.close();
   }
   else
   {
      printf("Error opening file %s.  Exiting...\n", fileName);
      exit(0);
   }

   return;
}

void vehicleT::displayVehicleInfo()
{
   int i;

   output("Vehicle name: %s\n", name);
   output("Number of sensors: %i\n\n", numSensors);

   for(i = 0; i < numSensors; i++)
   {
      output("Sensor #%i: \n", i+1);
      sensors[i].displaySensorInfo();

      output("Sensor #%i to vehicle transformation information: \n", i+1);
      T_sv[i].displayTransformInfo();
      output("\n");
   }

}
