#define POSIX_SOURCE

#include "Cins.h"

#include "CGetParam.h"

#include <stdio.h>
#include <signal.h>
#include <time.h>
#include <fstream>

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

// cycle times in seconds for threads
#define C_INS_BITDATA_CYCLETIME  5
#define C_INS_READPOSITIONDATA_CYCLETIME 60
#define C_Max_Roll_Before_Out_Of_Level (1.0 * C_DEG_TO_RAD)

// define global variables that hold the local values

bool g_request_antenna_orientation=false;
bool g_request_new_antenna_orientation=false;
bool g_turn_antenna_to_orp=false;

bool g_override_ins=false;


bool g_insDruNeedsZeroVelocityUpdate=false;
bool g_insGetDruNeedsPositionUpdate=false;
bool g_insGetBitMalFunction=false;
bool g_insGetDruAlert=false;
bool g_insGetDruFailure=false;
bool g_insGetVmsFailure=false;
bool g_insGetVmsDriveFailure=false;

bool g_insLinkError=false;


float g_azim; //radians
float g_tilt; //radians
float g_roll; //radians

float g_vazim; //radians/s
float g_vtilt; //radians/s
float g_vroll; //radians/s


float g_new_azim; //radians
float g_new_tilt; //radians
float g_new_roll; //radians


float g_orp_azim=0.0; //radians
float g_orp_tilt=0.0; //radians


float g_conv_azim;
float g_conv_tilt;
float g_conv_new_azim;


char g_insdevice[30];


bool g_bStartINSPosThread=true;
bool g_bStartINSAttThread=true;
bool g_bStartINSBitThread=true;
bool g_bSendIPC=true;
bool g_bdelta_mode=false;
bool g_bRoll_ok=true;

bool g_bLogInsDataToFile=false;


// debug definitions
#define C_ECOM_COM 0
#define C_ECOM_PLUS 1
#define C_ECOM_20HZ_LOOP 2
#define C_ECOM_OVERRUN 3
#define C_INS 4
#define C_TTU 5
#define C_THREAD_STATE 6
#define C_HANGUP 7

bool trace_level[C_MAX_NO_OF_TRACE_LEVELS];
m_trace_levels_menu trace_menu;
int g_INSPositionData_State=0;
int g_INSBITStatusData_State=0;
int g_INSAttitudeData_State=0;

//File for logging of ant ori
ofstream logInsDataFile;


bool g_antInUprightPosition = false;


struct thread_data{
   void *alpha;
   void *bravo;
   void *charlie;
};



static int readmutex;

static int sendMessage_mutex;



EXT_C_DECL float convertAzim(float x, float x1)
{
  float retval;

  retval = x - x1;

  if (trace_level[C_ECOM_20HZ_LOOP])
      printf("main::convertAzim retval1=%f ", retval);

  if ((x1 >= 0.0) && (x1 <= C_PI)) {
    if (retval >  C_PI) retval=retval-C_TWO_PI;
  } else {
    if (retval < -C_PI) retval=retval+C_TWO_PI;
  }

  if (trace_level[C_ECOM_20HZ_LOOP])
      printf("- x=%f, x.orp=%f, retval2=%f\n", x, x1, retval);

  return retval;

}

EXT_C_DECL float convertTilt(float x)
{
  float retval;

  retval = C_PI_HALF - x;

  if (trace_level[C_ECOM_PLUS])
      printf("main::convertTilt x=%f, retval=%f\n", x, retval);

  return retval;

}





///////////////////////////////////////////
//
//   This thread ask INS for the status and
//
///////////////////////////////////////
EXT_C_DECL void *INSReadStatus(void *pvalue)
{
  Cins *pins;
  bool newVal;
  bool gotData;

  pins=(Cins *)pvalue;

  while(1)
    {
      g_INSBITStatusData_State = 1;

      if (trace_level[C_ECOM_COM] || trace_level[C_ECOM_PLUS])
	{
	  printf("main::Calling INS_GetBitData\n");
	}
      g_INSBITStatusData_State = 2;
      gotData = pins->INS_GetBitData();

      if (gotData)
        {
          g_INSBITStatusData_State = 3;
          newVal=pins->INS_GetDruNeedsZeroVelocityUpdate();

        }
      else
        {
          // Did not receive any attitude data
          if (trace_level[C_ECOM_PLUS])
            printf("Bit Data not found\n");

        }
    }
}




EXT_C_DECL void *INSPositionData(void *pvalue)
{

  Cins *pins;


  unsigned short int Spheroid=0;
  short int Hemisphere=0;
  unsigned int Easting=0;
  unsigned int Northing=0;
  short int Altitude=0;
  bool gotData;


  pins=(Cins *)pvalue;

  while(1)
    {
      g_INSPositionData_State = 1;

      if (trace_level[C_ECOM_COM] || trace_level[C_ECOM_PLUS])
	{
	  printf("main::Calling INS_GetPositionData\n");
	}
      g_INSPositionData_State = 2;
      gotData=pins->INS_GetPositionData(&Spheroid,&Hemisphere,&Easting,&Northing,&Altitude);

    }
}



EXT_C_DECL void *LoopData(void *threadarg)
{

  struct thread_data *my_data;

  my_data = (struct thread_data *) threadarg;

  void *temp_a, *temp_b, *temp_c;
  temp_a = my_data->alpha;
  temp_b = my_data->bravo;
  temp_c = my_data->charlie;

  int i=0;
  char *buf;

  int NumToDB=0;
  int NumToMTT=0;

  bool send_offset_msg = false;
  double delta_tetta;
  bool gotData;
  Cins *pins;

  double gotDataTime;

  pins=(Cins *)temp_a;



  // Goto decl/body works here
  pins->INS_SetDebug(trace_level[C_INS]);
  pins->INS_SetDebugHangup(trace_level[C_HANGUP]);


  while (1)
    {
      i++;
      if (i > 10000)
	{
	  i=0;
	}
      if (trace_level[C_ECOM_20HZ_LOOP])
	{
	  printf("INS loop tick %d\n",i);
	}

      g_INSAttitudeData_State = 1;


      g_INSAttitudeData_State = 2;

      // Goto decl/body doesn't work here
      gotData = pins->INS_GetPointingDeviceAttitudeData(&g_azim,&g_tilt,&g_roll);
      //gotData = pins-
    } //While(1)
}




EXT_C_DECL int main(void)
{

  int value;
  Cins* pins;


  /*pthread_t TTUReadCommandId;
  pthread_t ReadFromDPUId;
  pthread_t INSReadStatusId;
  pthread_t TTUSendStatusReqId;
  pthread_t INSPositionDataId;
  pthread_t LoopDataId;*/

  bool ttuComErrorReported=false;
  bool insComErrorReported=false;

  for(int i = 0; i < C_MAX_NO_OF_TRACE_LEVELS; i++)
    {
      trace_level[i]=false;
    }



  // initialize the mutexes
  /*pthread_mutex_init(&readmutex,NULL);
  pthread_mutex_init(&sendMessage_mutex, NULL);*/



  pins = new Cins(g_insdevice,false, &readmutex);

  thread_data Thread_data_main;

  Thread_data_main.alpha=pins;
  Thread_data_main.bravo=pins;
  Thread_data_main.charlie=pins;



  if(g_bStartINSBitThread)
    {
      //value=pthread_create(&INSReadStatusId,NULL,INSReadStatus,pins);
      if (value!=0) printf("main::Error in pthread_create INSReadStatus, value %d\n",value);
    }

  if(g_bStartINSPosThread)
    {
      //value=pthread_create(&INSPositionDataId,NULL,INSPositionData,pins);
      if (value!=0) printf("main::Error in pthread_create INSPositionData, value %d\n",value);
    }

  if(g_bStartINSAttThread)
    {
      //value=pthread_create(&LoopDataId,NULL,LoopData,(void *) &Thread_data_main);
      if (value!=0) printf("main::Error in pthread_create LoopData, value %d\n",value);
    }


  while (true) // loop forever
    {
      //sleep(1); // avoid high load

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

      t_position_status tmp5;
      t_position_status tmp6 = raising_or_folding;
      m_ant_position_status tmp7;

      t_weapon_type w;

      CGetParam *myParam = new CGetParam("file.dat");


      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);




      // Problems with Xref menu below
      // typedef struct
      tmp4.ant_ori.azim = 1;
      tmp4.time.min = 2;

      // typedef enum
      tmp5 = raising_or_folding;
      tmp7.position_status = tmp6;





      switch ( w.weapon_class )
	{
	  case tube       : break;
	  case rocket     : break;
	  case mortar     : break;
	  case small_arms : break;
	  case manual     : break;
	  default         : break;
	}











      delete myParam;


    }

}