
#include <QElapsedTimer>

#include "g346_a4a5.h"
#include "g346_common.h"

#include "ads_manager.h"

#include "time_metrics.h"

#include <math.h>

#include <QDebug>

#include "icd_g346.h"
using namespace grifo_icd;

static metricRegistered mA4UPD("A4UPD", 25000, 100, &G346Metric);
static metricRegistered mA5UPD("A5UPD", 25000, 100, &G346Metric);

class InuTimetagCheck: public Ads1553MessageCheck
{
public:
  uint16_t prev;
  int same;

  InuTimetagCheck()
  {
    info.name="INUTIMETAG";
    info.description="Timetag progression and sync";

    same=0;
    prev=0;
  }
  void execute(avb_1553_mon_decoded_t &decoded)
  {
    uint16_t tt=decoded.data[1];

    ++info.checks;

    if (tt<prev)
    {
      same=0;
      float tmp=prev*relative_timetag_t::lsb();
      if (tmp<4000.0)
      {
        ++info.fails;
        info.lastFailReason="<";
      }
    }
    else if (tt==prev)
    {
      ++same;
      if (same>2)
      {
        ++info.fails;
        info.lastFailReason="not changed";
      }
    }
    else
    {
      same=0;
    }
    info.durationTime=tt;
    info.delayTime=prev;
    prev=tt;
    updated(*this);

  }
};

GrifoMsg_A4A5::GrifoMsg_A4A5():
    inuTtCheck(*new InuTimetagCheck)
{
  Ads1553MsgQ* a4=Ads1553Manager::instance().getMsgObj("A4");
  Ads1553MsgQ* a5=Ads1553Manager::instance().getMsgObj("A5");

  //A4
  GRIFO_ADS_MAP(a4_timetag, "A4_Timetag", msg_nav_data_and_cursor_t, timetag, relative_timetag_t, *a4);
  GRIFO_ADS_MAP_SCALED(a4_true_heading, "A4_true_heading", msg_nav_data_and_cursor_t, true_heading, semicircle_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_magnetic_heading, "A4_magnetic_heading", msg_nav_data_and_cursor_t, magnetic_heading, semicircle_t, *a4, GC_K_RAD_TO_DEG);

  GRIFO_ADS_MAP(a4_crs_zero, "A4_crs_zero", msg_nav_data_and_cursor_t, validity_and_slew, crs_zero_valid_field_t, *a4);
  GRIFO_ADS_MAP(a4_crs_slave, "A4_crs_slave", msg_nav_data_and_cursor_t, validity_and_slew, crs_slave_selector_field_t, *a4);
  GRIFO_ADS_MAP(a4_crs_snowplough, "A4_crs_snowplough", msg_nav_data_and_cursor_t, validity_and_slew, crs_snowplough_valid_field_t, *a4);

  GRIFO_ADS_MAP(a4_crs_rate_valid, "A4_crs_rate_val", msg_nav_data_and_cursor_t, validity_and_slew, crs_rate_invalid_field_t, *a4);

  GRIFO_ADS_MAP(a4_crs_rate_x, "A4_crs_x", msg_nav_data_and_cursor_t, acq_crs_x, crs_motion_rate_field_t, *a4);
  GRIFO_ADS_MAP(a4_crs_rate_y, "A4_crs_y", msg_nav_data_and_cursor_t, acq_crs_y, crs_motion_rate_field_t, *a4);

  GRIFO_ADS_MAP_SCALED(a4_crs_azimuth, "A4_crs_az", msg_nav_data_and_cursor_t, acq_crs_az, semicircle_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_crs_range, "A4_crs_rng", msg_nav_data_and_cursor_t, acq_crs_rng, crs_slave_range_t, *a4, GC_K_METERS_TO_FEETS);

  GRIFO_ADS_MAP(a4_ant_slew, "A4_ant_slew", msg_nav_data_and_cursor_t, validity_and_slew, ant_slew_valid_field_t, *a4);
  GRIFO_ADS_MAP_SCALED(a4_az_demand, "A4_az_demand", msg_nav_data_and_cursor_t, az_antenna_demand, semicircle_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_el_demand, "A4_el_demand", msg_nav_data_and_cursor_t, el_antenna_demand, semicircle_t, *a4, GC_K_RAD_TO_DEG);

  GRIFO_ADS_MAP_SCALED(a4_x_acc, "A4_x_acc", msg_nav_data_and_cursor_t, x_acceleration, acceleration_t, *a4, GC_K_METERS_TO_FEETS);
  GRIFO_ADS_MAP_SCALED(a4_y_acc, "A4_y_acc", msg_nav_data_and_cursor_t, y_acceleration, acceleration_t, *a4, GC_K_METERS_TO_FEETS);
  GRIFO_ADS_MAP_SCALED(a4_z_acc, "A4_z_acc", msg_nav_data_and_cursor_t, z_acceleration, acceleration_t, *a4, GC_K_METERS_TO_FEETS);

  GRIFO_ADS_MAP(a4_spoi_pos_valid, "A4_spoi_pos_invalid", msg_nav_data_and_cursor_t, validity_and_slew, spoi_position_invalid_t, *a4);
  GRIFO_ADS_MAP(a4_spoi_altitude_valid, "A4_spoi_antitude_invalid", msg_nav_data_and_cursor_t, validity_and_slew, spoi_altitude_invalid_t, *a4);
  GRIFO_ADS_MAP_SCALED(a4_spoi_latitude, "A4_spoi_lat", msg_nav_data_and_cursor_t, spoi_lat, latlong_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_spoi_longitude, "A4_spoi_long", msg_nav_data_and_cursor_t, spoi_lon, latlong_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_spoi_altitude, "A4_spoi_baroalt", msg_nav_data_and_cursor_t, spoi_baro_altitude, spoi_altitude_t, *a4, GC_K_METERS_TO_FEETS);  //LV

  GRIFO_ADS_MAP(a4_ppos_timetag, "A4_ppos_timetag", msg_nav_data_and_cursor_t, ppos_timetag, relative_timetag_t, *a4);
  GRIFO_ADS_MAP(a4_ppos_valid, "A4_ppos_invalid", msg_nav_data_and_cursor_t, validity_and_slew, present_position_invalid_t, *a4);
  GRIFO_ADS_MAP_SCALED(a4_ppos_latitude, "A4_ppos_lat", msg_nav_data_and_cursor_t, ppos_lat, latlong_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_ppos_longitude, "A4_ppos_long", msg_nav_data_and_cursor_t, ppos_lon, latlong_t, *a4, GC_K_RAD_TO_DEG);

  GRIFO_ADS_MAP_SCALED(a4_wind_direction, "A4_wind_direction", msg_nav_data_and_cursor_t, wind_direction, semicircle_t, *a4, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a4_wind_speed, "A4_wind_speed", msg_nav_data_and_cursor_t, wind_speed, velocity_t, *a4, GC_K_METERS_TO_FEETS);

  GRIFO_ADS_MAP_SCALED(a4_corrected_baro_altitude, "a4_corrected_baro_altitude", msg_nav_data_and_cursor_t, corrected_baro_altitude, baro_altitude_t, *a4, GC_K_METERS_TO_FEETS);
  GRIFO_ADS_MAP_SCALED(a4_radio_altimeter_altitude, "a4_radio_altimeter_altitude", msg_nav_data_and_cursor_t, radio_altitude, radio_altitude_t, *a4, GC_K_METERS_TO_FEETS);
  GRIFO_ADS_MAP_SCALED(a4_baro_altitude, "a4_baro_altitude", msg_nav_data_and_cursor_t, baro_altitude, baro_altitude_t, *a4, GC_K_METERS_TO_FEETS);

  GRIFO_ADS_MAP(a4_baro_altitude_invalid, "a4_baro_altitude_invalid", msg_nav_data_and_cursor_t, validity_and_slew, baro_intertial_altitude_invalid_t, *a4);
  GRIFO_ADS_MAP(a4_radio_altimeter_invalid, "a4_radio_altimeter_invalid", msg_nav_data_and_cursor_t, validity_and_slew, radio_altimeter_invalid_t, *a4);
  GRIFO_ADS_MAP(a4_corrected_baro_altitude_invalid, "a4_corrected_baro_altitude_invalid", msg_nav_data_and_cursor_t, validity_and_slew, correct_baro_altitude_invalid_t, *a4);

  GRIFO_ADS_MAP(a4_tas_invalid, "a4_tas_invalid", msg_nav_data_and_cursor_t, validity_and_slew, tas_invalid_t, *a4);
  GRIFO_ADS_MAP(a4_cas_invalid, "a4_cas_invalid", msg_nav_data_and_cursor_t, validity_and_slew, cas_invalid_t, *a4);
  GRIFO_ADS_MAP_SCALED(a4_tas, "a4_tas", msg_nav_data_and_cursor_t, true_air_speed, tas_t, *a4, GC_K_MS_TO_KNOTS);
  GRIFO_ADS_MAP_SCALED(a4_cas, "a4_cas", msg_nav_data_and_cursor_t, calibrated_air_speed, cas_t, *a4, GC_K_MS_TO_KNOTS);

  //A5
  GRIFO_ADS_MAP(a5_timetag, "A5_Timetag", msg_inu_high_speed_t, timetag, relative_timetag_t, *a5);
  GRIFO_ADS_MAP(a5_ptaz_timetag, "A5_ptaz_timetag", msg_inu_high_speed_t, ptaz_tt, relative_timetag_t, *a5);
  GRIFO_ADS_MAP(a5_pitch_timetag, "A5_pitch_timetag", msg_inu_high_speed_t, pitch_tt, relative_timetag_t, *a5);
  GRIFO_ADS_MAP(a5_roll_timetag, "A5_roll_timetag", msg_inu_high_speed_t, roll_tt, relative_timetag_t, *a5);


  GRIFO_ADS_MAP_SCALED(a5_ptaz, "A5_ptaz", msg_inu_high_speed_t, platform_azimuth, semicircle_t, *a5, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a5_pitch, "A5_pitch", msg_inu_high_speed_t, pitch, semicircle_t, *a5, GC_K_RAD_TO_DEG);
  GRIFO_ADS_MAP_SCALED(a5_roll, "A5_roll", msg_inu_high_speed_t, roll, semicircle_t, *a5, GC_K_RAD_TO_DEG);

  GRIFO_ADS_MAP_SCALED(a5_pitch_rate, "A5_rate_pitch", msg_inu_high_speed_t, pitch_rate, attitude_rate_t, *a5, GC_K_RAD_TO_DEG);   //LV
  GRIFO_ADS_MAP_SCALED(a5_roll_rate, "A5_rate_roll", msg_inu_high_speed_t, roll_rate, attitude_rate_t, *a5, GC_K_RAD_TO_DEG);      //LV
  GRIFO_ADS_MAP_SCALED(a5_yaw_rate, "A5_rate_yaw", msg_inu_high_speed_t, yaw_rate, attitude_rate_t, *a5, GC_K_RAD_TO_DEG);         //LV

  GRIFO_ADS_MAP(a5_pitch_ang_acc, "A5_angacc_pitch", msg_inu_high_speed_t, pitch_ang_acc, attitude_rate_t, *a5);                   //LV
  GRIFO_ADS_MAP(a5_roll_ang_acc, "A5_angacc_roll", msg_inu_high_speed_t, roll_ang_acc, attitude_rate_t, *a5);                      //LV
  GRIFO_ADS_MAP(a5_yaw_ang_acc, "A5_angacc_yaw", msg_inu_high_speed_t, yaw_ang_acc, attitude_rate_t, *a5);                         //LV

  GRIFO_ADS_MAP_SCALED(a5_x_vel, "A5_x_vel", msg_inu_high_speed_t, x_vel, velocity_t, *a5, GC_K_METERS_TO_FEETS);
  GRIFO_ADS_MAP_SCALED(a5_y_vel, "A5_y_vel", msg_inu_high_speed_t, y_vel, velocity_t, *a5, GC_K_METERS_TO_FEETS);
  GRIFO_ADS_MAP_SCALED(a5_z_vel, "A5_z_vel", msg_inu_high_speed_t, z_vel, velocity_t, *a5, GC_K_METERS_TO_FEETS);

  GRIFO_ADS_MAP(a5_nx, "a5_nx", msg_inu_high_speed_t, longitudinal_acc, acceleration_t, *a5);
  GRIFO_ADS_MAP(a5_ny, "a5_ny", msg_inu_high_speed_t, lateral_acc, acceleration_t, *a5);
  GRIFO_ADS_MAP(a5_nz, "a5_nz", msg_inu_high_speed_t, normal_acc, acceleration_t, *a5);

  GRIFO_ADS_MAP(a4_present_position_invalid, "PRESENT_POSITION_INVALID", msg_nav_data_and_cursor_t, validity_and_slew, present_position_invalid_t, *a4);

  GRIFO_ADS_MAP(a4_attitude_invalid, "ATTITUDE_INVALID", msg_nav_data_and_cursor_t, validity_and_slew, attitude_invalid_t, *a4);

  GRIFO_ADS_MAP(a4_navigation_data_invalid, "NAVIGATION_DATA_INVALID", msg_nav_data_and_cursor_t, validity_and_slew, navigation_invalid_t, *a4);

  GRIFO_ADS_MAP(a4_clearance_plane_dist, "CLEARANCE_PLANE_DIST", msg_nav_data_and_cursor_t, clearance_plane_distance, clearance_plane_distance_t, *a4);

  GRIFO_ADS_MAP(a4_sar_enabled, "SAR_ENABLED", msg_nav_data_and_cursor_t, acq_crs_x, crs_sar_selector_field_t, *a4);
  GRIFO_ADS_MAP(a4_norm_ghost_selection, "NORM_GHOST_SELECTION", msg_nav_data_and_cursor_t, acq_crs_x, crs_ghost_selector_field_t, *a4);
  GRIFO_ADS_MAP(a4_dtt_enabled, "DTT_ENABLED", msg_nav_data_and_cursor_t, acq_crs_x, crs_dtt_selector_field_t, *a4);


  a5_x_vel.setFromUser(400.0);
  a4_z_acc.setFromUser(32.2572);

  a5->addDataCheck(inuTtCheck);
}

void GrifoMsg_A4A5::clear_antenna_slew()
{
    a4_ant_slew.setFromUser((int)0);
}

void GrifoMsg_A4A5::set_antenna_el(bool valid, double el)
{
  if (!valid)
    a4_ant_slew.setFromUser((int)0);
  a4_el_demand.setFromUser(el);
  if (valid)
    a4_ant_slew.setFromUser((int)1);
}

void GrifoMsg_A4A5::move_antenna_el(bool valid, double el)
{
  double v=a4_el_demand.toDouble();
  v+=el;
  if (v>61)
      return;
  if (v<-61)
      return;
  set_antenna_el(valid, v);
  /*
  if (!valid)
    a4_ant_slew.setFromUser((int)0);
  double v=a4_el_demand.toDouble();
  v+=el;
  a4_el_demand.setFromUser(v);
  if (valid)
    a4_ant_slew.setFromUser((int)1);
  */
}

void GrifoMsg_A4A5::move_cursor(bool valid, int rate_x, int rate_y)
{
  if (!valid)
    a4_crs_rate_valid.setFromUser((int)0);

  a4_crs_rate_x.setFromUser(rate_x);
  a4_crs_rate_y.setFromUser(rate_y);

  if (valid)
    a4_crs_rate_valid.setFromUser((int)1);
}

void GrifoMsg_A4A5::cursorToSpoi()
{
    static Ads1553Manager& mng=Ads1553Manager::instance();
    static Ads1553MsgQ* msgB6=mng.getMsgObj("B6");
    static Ads1553MsgQ* msgA4=mng.getMsgObj("A4");

    msgA4->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, spoi_lat), msgB6->data32(ICD_WOFFSETOF(msg_settings_tellback_t, crs_lat)));
    msgA4->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, spoi_lon), msgB6->data32(ICD_WOFFSETOF(msg_settings_tellback_t, crs_long)));
}

static float a5_vx_;
static float a5_vy_;

void GrifoMsg_A4A5::a5_asynchronous_message_routine(Ads1553MsgQ* msg)
{
    static QElapsedTimer elapsedTime;

  static Ads1553Manager& mng=Ads1553Manager::instance();
  unsigned int tt=mng.getTimetag();

  static Ads1553MsgQ* msgA4=mng.getMsgObj("A4");

  metricMeasure xx(mA5UPD);

  tt/=64;
  unsigned short tt16=static_cast<unsigned short>(tt);

  if (!grifo_sim_enable.a4_inhibit_timetag)
  {
    msg->setDataNoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, timetag), tt16+grifo_sim_enable.tt_offset);
    msg->setDataNoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, ptaz_tt), tt16+grifo_sim_enable.ttptaz_offset);
    msg->setDataNoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, roll_tt), tt16+grifo_sim_enable.ttroll_offset);
    msg->setDataNoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, pitch_tt), tt16+grifo_sim_enable.ttpitch_offset);
  }


  //some simulation for test
  //if (grifo_sim_enable.ac!=avb_term_simulated)
  //  return;


  if (grifo_sim_enable.inu_smart)
  {
      double elapsed_seconds=elapsedTime.isValid() ? (float(elapsedTime.elapsed())/1000.0f) : 0.020;

      short ptaz=msg->data(ICD_WOFFSETOF(msg_inu_high_speed_t, platform_azimuth));

      float ptaz_rad=(ptaz)*semicircle_t::lsb();

      if (grifo_sim_enable.simple_sim)
      {
          short ptaz_var=msg->data(ICD_WOFFSETOF(msg_inu_high_speed_t, yaw_rate));
          if (ptaz_var)
          {
            float ptaz_var_rad=ptaz_var*attitude_rate_t::lsb();
            float ptaz_rate=ptaz_var_rad; //grifo_sim_enable.ptaz_variation;
            //ptaz_rate=gc_math::deg_to_rad(ptaz_rate);
            ptaz_rate*=elapsed_seconds; //0.020;
            ptaz_rad=gc_math::angle_bound_pi(ptaz_rad+(-ptaz_rate));
            ptaz=ptaz_rad/semicircle_t::lsb();
            msg->setDataNoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, platform_azimuth), ptaz);
          }
      }

      float magnetic_heading_rad=gc_math::angle_bound_pi(ptaz_rad+grifo_sim_enable.magnetic_variation);
      float true_heading_rad=gc_math::angle_bound_pi(ptaz_rad+grifo_sim_enable.wander_angle);

      short magnetic_heading=magnetic_heading_rad/semicircle_t::lsb();
      short true_heading=true_heading_rad/semicircle_t::lsb();

      //msg->setDataNoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, platform_azimuth), ptaz_rad/semicircle_t::lsb());

      msgA4->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, magnetic_heading), magnetic_heading);
      msgA4->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, true_heading), true_heading);

      short i_speed=msgA4->data(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, true_air_speed));

      unsigned int cas=(float(i_speed)*tas_t::lsb())/cas_t::lsb();
      msgA4->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, calibrated_air_speed), cas<<6);

      unsigned int h=msgA4->data(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, baro_altitude));
      msgA4->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, corrected_baro_altitude), h);

      unsigned int radio_altitude=(h*baro_altitude_t::lsb())/radio_altitude_t::lsb();
      msgA4->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, radio_altitude), radio_altitude);

      float speed=i_speed*tas_t::lsb();

      float angle=-ptaz_rad;

      //angle+=gc_math::angle_bound_pi(gc_math::deg_to_rad(grifo_sim_enable.nav_drift));
      angle=gc_math::angle_bound_pi(angle+grifo_sim_enable.nav_drift);

      float vx=speed*cos(angle);
      float vy=speed*sin(angle);

      int vx32=vx/velocity_t::lsb();
      int vy32=vy/velocity_t::lsb();

      msg->setData32NoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, x_vel), vx32);
      msg->setData32NoSignal(ICD_WOFFSETOF(msg_inu_high_speed_t, y_vel), vy32);

      a5_vx_=vx;
      a5_vy_=vy;
  }
  elapsedTime.restart();
}

static void lat_lon_pp(double& LAT_pp, double& LON_pp, float Vx, float Vy, float delta_t)
{
#if 0
    float Xc=Vx*delta_t;
    float Yc=Vy*delta_t;

    const float F_WGS84 = 0.003352810;
    const float R_WGS84 = 6378131.6341;

    float RNGc = sqrtf( Xc*Xc + Yc*Yc );
    float PHIc = atan2( Yc , Xc );

    float sin_lat=sinf(LAT_pp);
    float sin_lat2=sin_lat*sin_lat;

    float REast = R_WGS84 * (1 + F_WGS84*(sin_lat2) );
    float RNorth = R_WGS84* ( 1 - F_WGS84*(2-3*(sin_lat2) ));

    float LATc =  LAT_pp + cosf(- PHIc) * RNGc / RNorth;
    float LONc =  LON_pp + sinf(- PHIc) * RNGc / (REast*cosf(LAT_pp));

    LAT_pp = LATc;
    LON_pp = LONc;
#else
    Vy=-Vy;
    double Xc=Vx*delta_t;
    double Yc=Vy*delta_t;

    //const double F_WGS84 = 0.003352810;
    const double R_WGS84 = 6378131.6341;

    double RNGc = sqrt( Xc*Xc + Yc*Yc );
    double PHIc = atan2( Yc , Xc );

    double angular_distance=RNGc/R_WGS84;
    double sin_lat=sin(LAT_pp);
    double cos_lat=cos(LAT_pp);
    double sin_angular_distance=sin(angular_distance);
    double cos_angular_distance=cos(angular_distance);
    double sin_bearing=sin(PHIc);
    double cos_bearing=cos(PHIc);
    double sin_lat2=sin_lat*cos_angular_distance+cos_lat*sin_angular_distance*cos_bearing;
    double asin_lat2=asin(sin_lat2);
    double y=sin_bearing*sin_angular_distance*cos_lat;
    double x=cos_angular_distance-sin_lat*sin_lat2;
    double d2=LON_pp+atan2(y, x);

    LAT_pp=asin_lat2;
    LON_pp=fmod(d2+(M_PI*3), M_PI*2)-M_PI;
#endif
}

static double v2d(double v)
{
    // 1nm = 1'
    v=v/1852;
    v=v*(1/60.0);
    return v*GC_K_DEG_TO_RAD;
}

void GrifoMsg_A4A5::a4_asynchronous_message_routine(Ads1553MsgQ* msg)
{
  static QElapsedTimer elapsedTime;

  static double lat;
  static double lon;

  static Ads1553Manager& mng=Ads1553Manager::instance();
  static Ads1553MsgQ* msgB6=mng.getMsgObj("B6");

  metricMeasure xx(mA4UPD);

  if (!grifo_sim_enable.a5_inhibit_timetag)
  {
    unsigned int tt=mng.getTimetag();
    tt/=64;
    unsigned short tt16=static_cast<unsigned short>(tt);
    msg->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, timetag), tt16+grifo_sim_enable.tt_offset);
    msg->setDataNoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_timetag), tt16+grifo_sim_enable.tt_offset);
  }

  if (grifo_sim_enable.link_spoi_to_cursor)
  {
    msg->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, spoi_lat), msgB6->data32(ICD_WOFFSETOF(msg_settings_tellback_t, crs_lat)));
    msg->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, spoi_lon), msgB6->data32(ICD_WOFFSETOF(msg_settings_tellback_t, crs_long)));
  }

  if ((grifo_sim_enable.inu_smart>1) && elapsedTime.isValid())
  {
      double elapsed_seconds=elapsedTime.nsecsElapsed()/1.0E9;

      elapsedTime.restart();

      lat_lon_pp(lat, lon, a5_vx_, a5_vy_, elapsed_seconds);

      long long p64=(lon/latlong_t::lsb());
      int p32=p64;
      msg->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lon), p32);

      //p32=msg->data32(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lat));
      p64=(lat/latlong_t::lsb());
      p32=p64;
      msg->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lat), p32);

#if 0
    double sx=a5_vx_*elapsed_seconds;
    double sy=a5_vy_*elapsed_seconds;

    // 1nm = 1'
    sx=v2d(sx);
    sy=v2d(sy);

    //int p32=msg->data32(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lon));
    lon+=sy;
    long long p64=(lon/latlong_t::lsb());
    int p32=p64;
    msg->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lon), p32);

    p32=msg->data32(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lat));
    lat+=sx;
    p64=(lat/latlong_t::lsb());
    p32=p64;
    msg->setData32NoSignal(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lat), p32);
#endif
  }
  else
  {
      if (grifo_sim_enable.inu_smart<2)
      {
        int p32=msg->data32(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lat));
        lat=p32*latlong_t::lsb();
        p32=msg->data32(ICD_WOFFSETOF(msg_nav_data_and_cursor_t, ppos_lon));
        lon=p32*latlong_t::lsb();
      }
      elapsedTime.restart();
  }

}
