#include "Cins.h"

#include <sys/time.h>
#include <stdio.h>

#include <errno.h>
#include <signal.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#ifdef __cplusplus
#define EXT_C_DECL extern "C"
#else
#define EXT_C_DEC
#endif


# include <iostream>
using namespace std;



// Debug
static int trace = 0;


static const char serial_name[10] = "dummy";


static int PthreadValue;
static Cins *p;


static int cond = 1;

static int insmutex;

static void *readmutex;


static char answer[ANSWER_LENGTH]; // global byte array for the INS messages
static int expected_answer_len;
static int answer_len;
// Make sure the complete message is received in Callback function


static char curr_answer[ANSWER_LENGTH]; // global byte array for current message(s) received
static int tot_pos = 0;
static bool complete = false;
static int pos = 0;



EXT_C_DECL void *insWatchDog(void* );

#define BITDATAANSWERSIZE 5
#define POINTINGDEVICEANSWERSIZE 9
#define POSITIONDATAANSWERSIZE 13

// Statusbit definition
#define DRU_NEEDS_ZERO_VELOCITY_UPDATE 10
#define DRU_NEEDS_POSITION_UPDATE      9
#define BIT_MALFUNCTION_DETECTED       5
#define DRU_ALERT                      1

// Errorbit definition
#define VMS_DRIVE_FAILURE              0
#define VMS_FAILURE                    1
#define DRU_FAILURE                    15





static void Print_HexMsg(char * msg, int len)
{
  for (int i = 0; i < len; i++)
    {
      printf("0x%X ", msg[i]);
    }
  printf("\n");
}




static void Get_Message(char * tmp_answer, int tmp_size)
{

  for ( int i=0;i<tmp_size;i++ )
    {
      answer[pos] = tmp_answer[i];
      pos++;

      if ( pos == ANSWER_LENGTH )
	{
	  break;
	}

    } // for

  if ( pos == expected_answer_len )
    {
      if (trace) printf("Cins::Get_Message: Message complete.\n");
      complete = true;
      answer_len = pos;
      pos = 0;
      tot_pos = 0;
    }
  else if ( pos > expected_answer_len )
    {
      Print_HexMsg(answer, pos);
      complete = true;
      answer_len = -1;
      pos = 0;
      tot_pos = 0;
    }

};






// CLASS



void Cins::Serial_Callback(const char* signal,const char* bytes)
{
  char tmp_answer[ANSWER_LENGTH];
  int tmp_size = 0;





  if (complete == false)
    {
      memcpy(tmp_answer,curr_answer,tot_pos);

      tmp_size = strlen(bytes);
      memcpy(tmp_answer + tot_pos, bytes, tmp_size);

      tmp_size = tmp_size + tot_pos;
      tmp_answer[tmp_size] = '\0';
      tot_pos = tmp_size;
      if (trace) {
	printf("Callback_ins::Serial_Callback: We got a callback from ( %s ), size = %d, message = ",signal,tmp_size);
	Print_HexMsg(tmp_answer, tmp_size);
      }

      Get_Message(tmp_answer, tmp_size);

      // Signal
      if (complete == true)
	{

	  if (trace) printf("Callback_ins::Serial_Callback: Just sent signal.\n");
	}

    }
  else
    {
      tmp_size = strlen(bytes);
      memcpy(curr_answer + tot_pos, bytes, tmp_size);
      tot_pos = tmp_size + tot_pos;
      printf("Callback_ins::Serial_Callback: WARNING - message overflow! current message queue size = %d\n", tot_pos);
      Print_HexMsg(curr_answer, tot_pos);
    }



};



Cins::Cins(char *Device,bool ErrorReported, void *rmutex)
{
  int retval;
  bool bNRZI=false;

  m_ant_ttu_inactive tmp1;
  M_ANTENNA_IDENT tmp2;
  m_req_ant_position_status tmp3;
  m_ins_attitude_data tmp4;
  int rt, rp, ra;

  nummeasured=0;
  filterB=(1.0-FILTERA)/(1/20.0);  //precalculated value for velocity calculation

  readmutex = rmutex;

  //DeviceName=Device;
  InsUsed=false;
  bTimedOut=false;
  StopWatchDog=false;
  WatchDogSleepTime = 10;
  debug=0;
  debughangup=0;
  OpenErrorReported = ErrorReported;

  p=this;

  char buffer[20];
  myParam = new CGetParam("file.dat");


  // No problems with overloaded functions below
  myParam->GetString("GetString");

  myParam->GetString("GetString", false);

  retval = myParam->do_antenna(tmp1);

  retval = myParam->do_antenna(tmp2);

  retval = myParam->do_antenna(tmp3, rt, rp, ra);

  retval = myParam->do_antenna(tmp4);


  delete myParam;

//  retval = pthread_mutex_init(&insmutex,NULL);

  //PthreadValue=pthread_create(&insWatchDogId,NULL,insWatchDog,p);



  OpenErrorReported=false;

/*  if(pthread_mutex_unlock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  Constructor unlock              ********\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }

  if (OpenErrorReported == false)
    {
      printf("INS Constructor completed OK\n");
    } */

}



Cins::~Cins()
{

  StopWatchDog=true;

  //pthread_mutex_lock(&insmutex);
  //pthread_mutex_destroy(&insmutex);
  //pthread_cond_destroy(&cond);
  //pthread_exit (NULL);
  debug = 0;
}



void Cins::INS_SetDebug(bool newdebug)
{
  debug=newdebug;
}



void Cins::INS_SetDebugHangup(bool newdebug)
{
  debughangup=newdebug;
}



bool Cins::INS_ERROR(void)
{
  return bTimedOut;
}



bool Cins::INS_GetBitData(void)
{
  int *phelp;
  int help;
  int nwrote,ndata;;
  char chelp[4];
  bool success = false;
  char command[1];
  struct timespec ts1, ts2;
  ts1.tv_sec = 0;
  ts1.tv_nsec = INSBitLatency * 1000000;
  ts2.tv_sec = 0;
  ts2.tv_nsec = INSContextSwitchLatency * 1000;//Microseconds

 // printf("------ INS BIT: starting\n");
 /* if(pthread_mutex_lock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  INS_GetBitData lock error\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }*/

  if (trace) printf("\nCins::INS_GetBitData ...\n");

  command[0]=0x1f;
  nwrote=0;

  //nanosleep(&ts1,NULL);

  // Request new message
  complete = false;
  nwrote = Write_Serial(command, 1);
  if ((nwrote == -1) || (nwrote < 1))
    {
      printf("Cins::INS_GetBitData: Write_Serial failed!\n");
      //pthread_mutex_unlock(&insmutex);
      return success;
    }

  if (debug==1)
    printf("\nCins::INS_GetBitData - wrote %d bytes - waiting for answer ...\n", nwrote);

  //nanosleep(&ts2,NULL);

  if (debug==1)
    {
      ndata=INS_GetAnswer(BITDATAANSWERSIZE);
    }


  if(ndata > 0)
    {
      InsUsed=true;
    }
  else
    {
      InsUsed=false;
    }
  /*if(pthread_mutex_unlock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  INS_GetBitData unlock error\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }*/

  return success;
}



bool Cins::INS_GetPositionData(unsigned short int *Spheroid,
			       short int *Hemisphere,
			       unsigned int *Easting,
			       unsigned int *Northing,
			       short int *Altitude)
{
  int nwrote,ndata;
  int help;
  int *phelp;
  unsigned char chelp[4];
  short int *pshelp;
  short int shelp;
  bool success=false;
  char command[1];

  struct timespec ts1, ts2;
  ts1.tv_sec = 0;
  ts1.tv_nsec = INSPosLatency * 1000000;
  ts2.tv_sec = 0;
  ts2.tv_nsec = INSContextSwitchLatency * 1000;//Microseconds


  /*if(pthread_mutex_lock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  INS_GetPositionData lock error **********\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }*/

  if (trace) printf("\nCins::INS_GetPositionData ...\n");

  command[0]=0x09;
  nwrote=0;

  //nanosleep(&ts1,NULL);

  // Request new message
  complete = false;
  nwrote = Write_Serial(command, 1);
  if ((nwrote == -1) || (nwrote < 1))
    {
      printf("Cins::INS_GetPositionData: Write_Serial failed!\n");
      //pthread_mutex_unlock(&insmutex);
      return success;
    }

  if (debug==1)
    printf("\nCins::INS_GetPositionData - wrote %d bytes - waiting for answer ...\n", nwrote);

  //nanosleep(&ts2,NULL);

  ndata=INS_GetAnswer(POSITIONDATAANSWERSIZE);
  if (debug==1)
    {
      printf("INS: We got:");
      for (int i=0;i<ndata;i++)
	{
	  printf(" 0x%X",(unsigned char)answer[i]);
	}
      printf("\n");
    }


  if(ndata > 0)
    {
      InsUsed=true;
    }
  else
    {
      InsUsed=false;
    }
  /*if(pthread_mutex_unlock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  INS_GetPositionData unlock error ********\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }*/
  return success;
}




bool Cins::INS_GetPointingDeviceAttitudeData(float *azim,float *tilt,float *roll)
{

  unsigned short int Azimuth;
  short int Tilt;
  short int Roll;
  int nwrote,ndata;
  int help;
  int *phelp;
  unsigned char chelp[4];
  short int *pshelp;
  short unsigned int *psuhelp;
  bool success = false;
  char command[1];
  struct timespec ts1, ts2;
  ts1.tv_sec = 0;
  ts1.tv_nsec = INSAttLatency * 1000000;
  ts2.tv_sec = 0;
  ts2.tv_nsec = INSContextSwitchLatency * 1000;//Microseconds

  /*if(pthread_mutex_lock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  INS_GetPointingDeviceAttitudeData lock error\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }*/

  if (trace) printf("\nCins::INS_GetPointingDeviceAttitudeData ...\n");

  command[0]=0x0c;
  nwrote=0;

  //nanosleep(&ts1,NULL);

  // Request new message
  complete = false;
  nwrote = Write_Serial(command, 1);
  if ((nwrote == -1) || (nwrote < 1))
    {
      printf("Cins::INS_GetPointingDeviceAttitudeData: Write_Serial failed!\n");
      //pthread_mutex_unlock(&insmutex);
      return success;
    }

  if (debug==1)
    printf("\nCins::INS_GetPointingDeviceAttitudeData - wrote %d bytes - waiting for answer ...\n", nwrote);

  //nanosleep(&ts2,NULL);

  ndata=INS_GetAnswer(POINTINGDEVICEANSWERSIZE);
  if (debug==1)
    {
      printf("INS: We got:");
      for (int i=0;i<ndata;i++)
	{
	  printf(" 0x%X",(unsigned char)answer[i]);
	}
      printf("\n");
    }


  if(ndata > 0)
    {
      InsUsed=true;
    }
  else
    {
      InsUsed=false;
    }
  /*if(pthread_mutex_unlock(&insmutex) != 0)
    {
      printf("************************************************\n");
      printf("************************************************\n");
      printf("*****  INS_GetPointingDeviceAttitudeData unlock error\n");
      perror("*****:");
      printf("************************************************\n");
      printf("************************************************\n");
    }*/
  //pthread_mutex_unlock(&insmutex);
  return success;
}



void  Cins::INS_GetVelocityData(float *velazim,float *veltilt,float *velroll)
{
  *velazim=vazim;
  *veltilt=vtilt;
  *velroll=vroll;
}

bool Cins::INS_GetDruNeedsZeroVelocityUpdate(void)
{
  return StatusBit[DRU_NEEDS_ZERO_VELOCITY_UPDATE];
}

bool Cins::INS_GetDruNeedsPositionUpdate(void)
{
  return StatusBit[DRU_NEEDS_POSITION_UPDATE];
}

bool Cins::INS_GetBitMalFunction(void)
{
  return StatusBit[BIT_MALFUNCTION_DETECTED];
}

bool Cins::INS_GetDruAlert(void)
{
  return StatusBit[DRU_ALERT];
}

bool Cins::INS_GetDruFailure()
{
  return BitData[DRU_FAILURE];
}

bool Cins::INS_GetVmsFailure()
{
  return BitData[VMS_FAILURE];
}

bool Cins::INS_GetVmsDriveFailure()
{
  return BitData[VMS_DRIVE_FAILURE];
}



int Cins::INS_GetAnswer(int num)
{

  struct timeval now;
  struct timespec timeout;
  int retcode = 0;
  char tmp_answer[ANSWER_LENGTH];


  //pthread_mutex_lock(readmutex);

  gettimeofday(&now, NULL);
  timeout.tv_sec = now.tv_sec + 5;
  timeout.tv_nsec = now.tv_usec * 1000;

  expected_answer_len = num;

  if (complete == false)
    {
      if (trace) printf("Cins::INS_GetAnswer: Waiting ...\n");
      //retcode = pthread_cond_timedwait(&cond, readmutex, &timeout);
      if (trace) printf("Cins::INS_GetAnswer: Condition signal received.\n");
    }

  //if (retcode == ETIMEDOUT)
  //  {
  //    /* Timeout occurred */
  //    printf("Cins::INS_GetAnswer: TIMEOUT!\n");
  //  }

  if (complete == false)
    {
      answer_len = -1;
      // Reset receive buffer pointers
      pos = 0;
      tot_pos = 0;
    }

  if (trace) printf("Cins::INS_GetAnswer: Message received, size = %d\n", answer_len);

  //pthread_mutex_unlock(readmutex);

  return answer_len;

};


bool Cins::INS_GetOpenErrorReported(void)
{
 return OpenErrorReported;
}

void Cins::INS_GetStatusBit(unsigned short int Status)
{
  StatusBit[0]= ((Status & 1) == 1);
  StatusBit[1]= (((Status>>1) & 1) == 1);
  StatusBit[2]= (((Status>>2) & 1) == 1);
  StatusBit[3]= (((Status>>3) & 1) == 1);
  StatusBit[4]= (((Status>>4) & 1) == 1);
  StatusBit[5]= (((Status>>5) & 1) == 1);
  StatusBit[6]= (((Status>>6) & 1) == 1);
  StatusBit[7]= (((Status>>7) & 1) == 1);
  StatusBit[8]= (((Status>>8) & 1) == 1);
  StatusBit[9]= (((Status>>9) & 1) == 1);
  StatusBit[10]= (((Status>>10) & 1) == 1);
  StatusBit[11]= (((Status>>11) & 1) == 1);
  StatusBit[12]= (((Status>>12) & 1) == 1);
  StatusBit[13]= (((Status>>13) & 1) == 1);
  StatusBit[14]= (((Status>>14) & 1) == 1);
  StatusBit[15]= (((Status>>15) & 1) == 1);
}

void Cins::INS_GetBitData(unsigned short int Status)
{
  BitData[0]= ((Status & 1) == 1);
  BitData[1]= (((Status>>1) & 1) == 1);
  BitData[2]= (((Status>>2) & 1) == 1);
  BitData[3]= (((Status>>3) & 1) == 1);
  BitData[4]= (((Status>>4) & 1) == 1);
  BitData[5]= (((Status>>5) & 1) == 1);
  BitData[6]= (((Status>>6) & 1) == 1);
  BitData[7]= (((Status>>7) & 1) == 1);
  BitData[8]= (((Status>>8) & 1) == 1);
  BitData[9]= (((Status>>9) & 1) == 1);
  BitData[10]= (((Status>>10) & 1) == 1);
  BitData[11]= (((Status>>11) & 1) == 1);
  BitData[12]= (((Status>>12) & 1) == 1);
  BitData[13]= (((Status>>13) & 1) == 1);
  BitData[14]= (((Status>>14) & 1) == 1);
  BitData[15]= (((Status>>15) & 1) == 1);

}


EXT_C_DECL void *insWatchDog(void *value)
{
  Cins *p;
  p=(Cins *)value;

  // Wait until the system at least has tried to read position data once .....
  //sleep(60);

  while(p->StopWatchDog==false)
    {

      if (p->debug)
	{
	 if(p->InsUsed)
	   {
	     printf("ECOM_INS::WatchDog for INS - Status OK\n");
	   }
	 else
	   {
	     printf("ECOM_INS::WatchDog for INS - Status NOT OK\n");
	   }
	}

      // check for open link error
      //sleep(p->WatchDogSleepTime);
      if (p->InsUsed == false)
	{
	  p->bTimedOut=true;
	}
      else
	{
	  p->bTimedOut=false;
	}

    }
};



int Cins::Write_Serial(char * msg, int datalen)
{
  char value_buf[20];

  if (trace)
    {
      printf("Writing to serial channel ( %s )\n", serial_name);
      Print_HexMsg(msg, datalen);
    }

  memcpy(value_buf, msg, datalen);


  return datalen;

};