void dgnss_init_known_baseline2(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], double b[3])
{
  double ref_ecef[3];
  ref_ecef[0] = receiver_ecef[0] + 0.5 * b[0];
  ref_ecef[1] = receiver_ecef[1] + 0.5 * b[1];
  ref_ecef[2] = receiver_ecef[2] + 0.5 * b[2];

  sdiff_t corrected_sdiffs[num_sats];

  u8 old_prns[MAX_CHANNELS];
  memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8));
  //rebase globals to a new reference sat (permutes corrected_sdiffs accordingly)
  dgnss_rebase_ref(num_sats, sdiffs, ref_ecef, old_prns, corrected_sdiffs);

  double dds[2*(num_sats-1)];
  make_measurements(num_sats-1, corrected_sdiffs, dds);

  double DE[(num_sats-1)*3];
  s32 N[num_sats-1];
  assign_de_mtx(num_sats, corrected_sdiffs, ref_ecef, DE);
  amb_from_baseline(num_sats, DE, dds, b, N);

  printf("Known Base: [");
  for (u8 i=0; i<num_sats-1; i++)
    printf("%d, ", N[i]);
  printf("]\n");

  /* Construct fake state means. */
  double state_mean[num_sats-1+6];
  memcpy(&state_mean[0], b, 3 * sizeof(double));
  memset(&state_mean[3], 0, 3 * sizeof(double));
  for (u8 i=0; i<num_sats-1; i++) {
    state_mean[i+6] = N[i];
  }

  /* Construct fake covariance U factor (just identity). */
  double state_cov_U[(num_sats-1+6)*(num_sats-1+6)];
  eye(num_sats-1+6, state_cov_U);

  double state_cov_D[num_sats-1+6];
  memset(state_cov_D, 0, 6 * sizeof(double));
  for (u32 i=0; i<num_sats-1; i++) {
    state_cov_D[i+6] = 1.0 / 64.0;
  }

  dgnss_reset_iar();

  update_ambiguity_test(ref_ecef,
                        dgnss_settings.phase_var_test,
                        dgnss_settings.code_var_test,
                        &ambiguity_test,
                        num_sats-1+6, &sats_management, corrected_sdiffs,
                        state_mean, state_cov_U, state_cov_D);
}
void dgnss_init_known_baseline(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], double b[3])
{
  double ref_ecef[3];
  ref_ecef[0] = receiver_ecef[0] + 0.5 * b[0];
  ref_ecef[1] = receiver_ecef[1] + 0.5 * b[1];
  ref_ecef[2] = receiver_ecef[2] + 0.5 * b[2];

  sdiff_t corrected_sdiffs[num_sats];

  u8 old_prns[MAX_CHANNELS];
  memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8));
  //rebase globals to a new reference sat (permutes corrected_sdiffs accordingly)
  dgnss_rebase_ref(num_sats, sdiffs, ref_ecef, old_prns, corrected_sdiffs);

  double dds[2*(num_sats-1)];
  make_measurements(num_sats-1, corrected_sdiffs, dds);

  double DE[(num_sats-1)*3];
  assign_de_mtx(num_sats, corrected_sdiffs, ref_ecef, DE);

  dgnss_reset_iar();

  memcpy(&ambiguity_test.sats, &sats_management, sizeof(sats_management));
  hypothesis_t *hyp = (hypothesis_t *)memory_pool_add(ambiguity_test.pool);
  hyp->ll = 0;
  amb_from_baseline(num_sats, DE, dds, b, hyp->N);

  double obs_cov[(num_sats-1) * (num_sats-1) * 4];
  memset(obs_cov, 0, (num_sats-1) * (num_sats-1) * 4 * sizeof(double));
  u8 num_dds = num_sats-1;
  for (u8 i=0; i<num_dds; i++) {
    for (u8 j=0; j<num_dds; j++) {
      u8 i_ = i+num_dds;
      u8 j_ = j+num_dds;
      if (i==j) {
        obs_cov[i*2*num_dds + j] = dgnss_settings.phase_var_test * 2;
        obs_cov[i_*2*num_dds + j_] = dgnss_settings.code_var_test * 2;
      }
      else {
        obs_cov[i*2*num_dds + j] = dgnss_settings.phase_var_test;
        obs_cov[i_*2*num_dds + j_] = dgnss_settings.code_var_test;
      }
    }
  }

  init_residual_matrices(&ambiguity_test.res_mtxs, num_sats-1, DE, obs_cov);

  /*printf("Known Base: [");*/
  /*for (u8 i=0; i<num_sats-1; i++)*/
    /*printf("%d, ", hyp->N[i]);*/
  /*printf("]\n");*/
}
void dgnss_new_float_baseline(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3], u8 *num_used, double b[3])
{
  sdiff_t corrected_sdiffs[num_sats];

  u8 old_prns[MAX_CHANNELS];
  memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8));
  //rebase globals to a new reference sat (permutes corrected_sdiffs accordingly)
  dgnss_rebase_ref(num_sats, sdiffs, receiver_ecef, old_prns, corrected_sdiffs);

  double dd_measurements[2*(num_sats-1)];
  make_measurements(num_sats-1, corrected_sdiffs, dd_measurements);

  least_squares_solve_b(&nkf, corrected_sdiffs, dd_measurements, receiver_ecef, b);
  *num_used = sats_management.num_sats;
}
void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3], double dt)
{
  sdiff_t corrected_sdiffs[num_sats];

  u8 old_prns[MAX_CHANNELS];
  memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8));
  //rebase globals to a new reference sat (permutes corrected_sdiffs accordingly)
  dgnss_rebase_ref(num_sats, sdiffs, reciever_ecef, old_prns, corrected_sdiffs);

  double dd_measurements[2*(num_sats-1)];
  make_measurements(num_sats-1, corrected_sdiffs, dd_measurements);

  //all the added/dropped sat stuff
  dgnss_update_sats(num_sats, reciever_ecef, corrected_sdiffs, dd_measurements, dt);
  /*printf("done updating sats\n");*/
  /*MAT_PRINTF(kf.decor_obs_mtx, kf.obs_dim, kf.state_dim);*/

  if (num_sats >= 5) {
    // update for observation
    dgnss_incorporate_observation(corrected_sdiffs, dd_measurements, reciever_ecef, dt);
  }

  double b2[3];
  least_squares_solve_b(&nkf, corrected_sdiffs, dd_measurements, reciever_ecef, b2);

  double ref_ecef[3];
  ref_ecef[0] = reciever_ecef[0] + 0.5 * b2[0];
  ref_ecef[1] = reciever_ecef[1] + 0.5 * b2[1];
  ref_ecef[2] = reciever_ecef[2] + 0.5 * b2[2];

  /*
  update_ambiguity_test(ref_ecef,
                        dgnss_settings.phase_var_test,
                        dgnss_settings.code_var_test,
                        &ambiguity_test,
                        kf.state_dim, &sats_management, sdiffs,
                        kf.state_mean, kf.state_cov_U, kf.state_cov_D);
  */
  update_ambiguity_test(ref_ecef,
                        dgnss_settings.phase_var_test,
                        dgnss_settings.code_var_test,
                        &ambiguity_test, nkf.state_dim, &sats_management,
                        sdiffs, nkf.state_mean, nkf.state_cov_U,
                        nkf.state_cov_D);
}
void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3],
                  bool disable_raim, double raim_threshold)
{
  DEBUG_ENTRY();
  if (DEBUG) {
    printf("sdiff[*].prn = {");
    for (u8 i=0; i < num_sats; i++) {
      printf("%u, ", sdiffs[i].sid.sat);
    }
    printf("}\n");
  }

  if (num_sats <= 1) {
    sats_management.num_sats = num_sats;
    if (num_sats == 1) {
      sats_management.sids[0] = sdiffs[0].sid;
    }
    create_ambiguity_test(&ambiguity_test);
    DEBUG_EXIT();
    return;
  }

  if (sats_management.num_sats <= 1) {
    dgnss_init(num_sats, sdiffs, receiver_ecef);
  }

  sdiff_t sdiffs_with_ref_first[num_sats];

  gnss_signal_t old_sids[MAX_CHANNELS];
  memcpy(old_sids, sats_management.sids, sats_management.num_sats * sizeof(gnss_signal_t));

  /* rebase globals to a new reference sat
   * (permutes sdiffs_with_ref_first accordingly) */
  dgnss_rebase_ref(num_sats, sdiffs, receiver_ecef, old_sids, sdiffs_with_ref_first);

  double dd_measurements[2*(num_sats-1)];
  make_measurements(num_sats-1, sdiffs_with_ref_first, dd_measurements);

  /* all the added/dropped sat stuff */
  dgnss_update_sats(num_sats, receiver_ecef, sdiffs_with_ref_first, dd_measurements);

  /* Unless the KF says otherwise, DONT TRUST THE MEASUREMENTS */
  u8 is_bad_measurement = true;

  double ref_ecef[3];
  if (num_sats >= 5) {
    double b2[3];
    s8 code = least_squares_solve_b_external_ambs(nkf.state_dim, nkf.state_mean,
        sdiffs_with_ref_first, dd_measurements, receiver_ecef, b2,
        disable_raim, raim_threshold);

    if (code < 0) {
      log_warn("dgnss_update. baseline estimate error: %d", code);
      /* Use b = 0, continue */
      memset(b2, 0, sizeof(b2));
    }

    double ref_ecef[3];

    vector_add_sc(3, receiver_ecef, b2, 0.5, ref_ecef);

    /* TODO: make a common DE and use it instead. */

    set_nkf_matrices(&nkf,
                     dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf,
                     sats_management.num_sats, sdiffs_with_ref_first, ref_ecef);

    is_bad_measurement = nkf_update(&nkf, dd_measurements);
  }

  u8 changed_sats = ambiguity_update_sats(&ambiguity_test, num_sats, sdiffs,
                                          &sats_management, nkf.state_mean,
                                          nkf.state_cov_U, nkf.state_cov_D,
                                          is_bad_measurement);

  /* TODO: Refactor - looks like ref_ecef can be passed in uninitialized */
  if (!is_bad_measurement) {
    update_ambiguity_test(ref_ecef,
                          dgnss_settings.phase_var_test,
                          dgnss_settings.code_var_test,
                          &ambiguity_test, nkf.state_dim,
                          sdiffs, changed_sats);
  }

  update_unanimous_ambiguities(&ambiguity_test);

  DEBUG_EXIT();
}
void dgnss_update(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3])
{
  DEBUG_ENTRY();
  if (DEBUG) {
    printf("sdiff[*].prn = {");
    for (u8 i=0; i < num_sats; i++) {
      printf("%u, ",sdiffs[i].prn);
    }
    printf("}\n");
  }

  if (num_sats <= 1) {
    sats_management.num_sats = num_sats;
    if (num_sats == 1) {
      sats_management.prns[0] = sdiffs[0].prn;
    }
    create_ambiguity_test(&ambiguity_test);
    DEBUG_EXIT();
    return;
  }

  if (sats_management.num_sats <= 1) {
    dgnss_init(num_sats, sdiffs, reciever_ecef);
  }

  sdiff_t sdiffs_with_ref_first[num_sats];

  u8 old_prns[MAX_CHANNELS];
  memcpy(old_prns, sats_management.prns, sats_management.num_sats * sizeof(u8));

  /* rebase globals to a new reference sat
   * (permutes sdiffs_with_ref_first accordingly) */
  dgnss_rebase_ref(num_sats, sdiffs, reciever_ecef, old_prns, sdiffs_with_ref_first);

  double dd_measurements[2*(num_sats-1)];
  make_measurements(num_sats-1, sdiffs_with_ref_first, dd_measurements);

  /* all the added/dropped sat stuff */
  dgnss_update_sats(num_sats, reciever_ecef, sdiffs_with_ref_first, dd_measurements);

  double ref_ecef[3];
  if (num_sats >= 5) {
    dgnss_incorporate_observation(sdiffs_with_ref_first, dd_measurements, reciever_ecef);

    double b2[3];
    least_squares_solve_b(&nkf, sdiffs_with_ref_first, dd_measurements, reciever_ecef, b2);

    ref_ecef[0] = reciever_ecef[0] + 0.5 * b2[0];
    ref_ecef[1] = reciever_ecef[1] + 0.5 * b2[1];
    ref_ecef[2] = reciever_ecef[2] + 0.5 * b2[2];
  }

  u8 changed_sats = ambiguity_update_sats(&ambiguity_test, num_sats, sdiffs,
                                          &sats_management, nkf.state_mean,
                                          nkf.state_cov_U, nkf.state_cov_D);

  update_ambiguity_test(ref_ecef,
                        dgnss_settings.phase_var_test,
                        dgnss_settings.code_var_test,
                        &ambiguity_test, nkf.state_dim,
                        sdiffs, changed_sats);

  update_unanimous_ambiguities(&ambiguity_test);

  if (DEBUG) {
    if (num_sats >=4) {
      double b3[3];
      least_squares_solve_b(&nkf, sdiffs_with_ref_first, dd_measurements, reciever_ecef, b3);

      ref_ecef[0] = reciever_ecef[0] + 0.5 * b3[0];
      ref_ecef[1] = reciever_ecef[1] + 0.5 * b3[1];
      ref_ecef[2] = reciever_ecef[2] + 0.5 * b3[2];
      double bb[3];
      u8 num_used;
      dgnss_fixed_baseline(num_sats, sdiffs, ref_ecef,
                           &num_used, bb);
      log_debug("\ndgnss_fixed_baseline:\nb = %f, \t%f, \t%f\nnum_used/num_sats = %u/%u\nusing_iar = %u\n\n",
             bb[0], bb[1], bb[2],
             num_used, num_sats,
             ambiguity_iar_can_solve(&ambiguity_test));
    }
  }
  DEBUG_EXIT();
}