// SSR decoder harness for AddressSanitizer/UBSan runs (see ssr_harness.sh).
//
// Decodes every CRC-valid RTCM3 frame of a file with each of the three SSR
// format classes (SsrCorrIgs, SsrCorrRtcm, SsrCorrRtcmNew). Unlike in BNC,
// where all target structs are members of one RTCM3coDecoder object, every
// struct is a separate heap block here, so a write beyond a struct is
// reported by AddressSanitizer instead of silently corrupting the next member.
//
// Every decoded VTEC (all formats), metadata and satellite antenna message
// (new RTCM-SSR) is encoded again with the Make* function of the same class
// and decoded once more; the result must be identical, for the antenna
// messages also the bytes of the frame (exit code 1 otherwise).
//
// Usage: ssr_harness <file>   (BNC raw file or plain RTCM3 byte stream)

#include <cstdio>
#include <vector>

#include "clock_orbit_igs.h"
#include "clock_orbit_rtcm.h"
#include "clock_orbit_rtcm_new.h"

using namespace std;

// from bncutils.cpp, which would pull in most of BNC
unsigned long CRC24(long size, const unsigned char *buf) {
  unsigned long crc = 0;
  while (size--) {
    crc ^= (*buf++) << (16);
    for (int ii = 0; ii < 8; ii++) {
      crc <<= 1;
      if (crc & 0x1000000) {
        crc ^= 0x01864cfb;
      }
    }
  }
  return crc;
}

struct t_target {
  t_target()  : co(new SsrCorr::ClockOrbit()), cb(new SsrCorr::CodeBias()),
                v(new SsrCorr::VTEC()), pb(new SsrCorr::PhaseBias()),
                md(new SsrCorr::MetaData()), a(new SsrCorr::Antenna()) {}
  ~t_target() {delete co; delete cb; delete v; delete pb; delete md; delete a;}
  int decode(SsrCorr* ssr, const unsigned char* frame, size_t size) {
    int bytesused = 0;
    return ssr->GetSSR(co, cb, v, pb, md, a, (const char*)frame, size, &bytesused);
  }
  SsrCorr::ClockOrbit* co;
  SsrCorr::CodeBias*   cb;
  SsrCorr::VTEC*       v;
  SsrCorr::PhaseBias*  pb;
  SsrCorr::MetaData*   md;
  SsrCorr::Antenna*    a;
};

static bool sameVTEC(const SsrCorr::VTEC* v1, const SsrCorr::VTEC* v2) {
  if (v1->EpochTime != v2->EpochTime || v1->NumLayers != v2->NumLayers ||
      v1->Quality != v2->Quality) {
    return false;
  }
  for (unsigned l = 0; l < v1->NumLayers; l++) {
    const SsrCorr::VTEC::IonoLayers& l1 = v1->Layers[l];
    const SsrCorr::VTEC::IonoLayers& l2 = v2->Layers[l];
    if (l1.Height != l2.Height || l1.Degree != l2.Degree || l1.Order != l2.Order) {
      return false;
    }
    for (unsigned d = 0; d <= l1.Degree; d++) {
      for (unsigned o = 0; o <= l1.Order; o++) {
        if (l1.Cosinus[d][o] != l2.Cosinus[d][o] || l1.Sinus[d][o] != l2.Sinus[d][o]) {
          return false;
        }
      }
    }
  }
  return true;
}

static bool sameMetaData(const SsrCorr::MetaData* m1, const SsrCorr::MetaData* m2) {
  if (m1->SSRIOD != m2->SSRIOD || m1->SSRProviderID != m2->SSRProviderID ||
      m1->SSRSolutionID != m2->SSRSolutionID || m1->NumEntries != m2->NumEntries) {
    return false;
  }
  for (unsigned i = 0; i < m1->NumEntries; i++) {
    if (memcmp(&m1->Entries[i], &m2->Entries[i], sizeof(m1->Entries[i])) != 0) {
      return false;
    }
  }
  return true;
}

static int nFailed = 0;

// Decode, print a summary, encode the result again and compare
static void decode(const char* name, SsrCorr* ssr, const unsigned char* frame, size_t size) {
  t_target t1;
  int irc = t1.decode(ssr, frame, size);
  printf("  %-8s irc %3d", name, irc);
  SsrCorrRtcmNew* rtcmNew = dynamic_cast<SsrCorrRtcmNew*>(ssr);
  if (irc >= 0 && t1.v->NumLayers) {
    double sum = 0.0;
    printf("  VTEC");
    for (unsigned l = 0; l < t1.v->NumLayers; l++) {
      const SsrCorr::VTEC::IonoLayers& layer = t1.v->Layers[l];
      printf(" %u/%u", layer.Degree, layer.Order);
      for (unsigned d = 0; d <= layer.Degree; d++) {
        for (unsigned o = 0; o <= layer.Order; o++) {
          sum += layer.Cosinus[d][o] - layer.Sinus[d][o];
        }
      }
    }
    printf("  sum(C-S) %.3f", sum);

    char buffer[CLOCKORBIT_BUFFERSIZE];
    size_t len = ssr->MakeVTEC(t1.v, 0, buffer, sizeof(buffer));
    t_target t2;
    bool ok = len > 0 && t2.decode(ssr, (const unsigned char*)buffer, len) >= 0 && sameVTEC(t1.v, t2.v);
    printf("  re-encoded: %s", ok ? "identical" : "DIFFERENT");
    nFailed += !ok;
  }
  if (irc >= 0 && t1.md->NumEntries) {
    printf("  META %u entries, additional data", t1.md->NumEntries);
    for (unsigned i = 0; i < t1.md->NumEntries; i++) {
      if (t1.md->Entries[i].additionalParameterIndicator) {
        printf(" %u:0x%x", t1.md->Entries[i].additionalParameterNumBits,
               t1.md->Entries[i].additionalParameterData);
      }
    }
    if (rtcmNew) {
      char buffer[CLOCKORBIT_BUFFERSIZE];
      size_t len = rtcmNew->MakeMetaData(t1.md, buffer, sizeof(buffer));
      t_target t2;
      bool ok = len > 0 && t2.decode(ssr, (const unsigned char*)buffer, len) >= 0 && sameMetaData(t1.md, t2.md);
      printf("  re-encoded: %s", ok ? "identical" : "DIFFERENT");
      nFailed += !ok;
    }
  }
  for (unsigned s = 0; rtcmNew && irc >= 0 && s < CLOCKORBIT_SATNUM; s++) {
    if (!t1.a->SatelliteMask[s]) {
      continue;
    }
    printf("  ANT type %u mask %016llx", t1.a->messageType[s],
           (unsigned long long)t1.a->SatelliteMask[s]);
    char buffer[CLOCKORBIT_BUFFERSIZE];
    size_t len = rtcmNew->MakeAntenna(t1.a, t1.a->messageType[s], buffer, sizeof(buffer));
    t_target t2;
    bool sameBytes = len == size && memcmp(buffer, frame, size) == 0;
    bool ok = len > 0 && t2.decode(ssr, (const unsigned char*)buffer, len) >= 0 &&
              memcmp(t1.a, t2.a, sizeof(*t1.a)) == 0;
    printf("  re-encoded: %s, %s", ok ? "identical" : "DIFFERENT",
           sameBytes ? "same bytes" : "OTHER BYTES");
    nFailed += !ok || !sameBytes;
  }
  printf("\n");
}

int main(int argc, char* argv[]) {
  if (argc != 2) {
    fprintf(stderr, "usage: %s <file>\n", argv[0]);
    return 2;
  }
  FILE* inp = fopen(argv[1], "rb");
  if (!inp) {
    perror(argv[1]);
    return 2;
  }
  vector<unsigned char> buf;
  int c;
  while ((c = fgetc(inp)) != EOF) {
    buf.push_back((unsigned char)c);
  }
  fclose(inp);

  SsrCorrIgs     igs;
  SsrCorrRtcm    rtcm;
  SsrCorrRtcmNew rtcmNew;

  size_t ii = 0;
  while (ii + 6 <= buf.size()) {
    size_t len = ((buf[ii + 1] & 0x03) << 8) | buf[ii + 2];
    if (buf[ii] != 0xD3 || ii + len + 6 > buf.size() ||
        CRC24(len + 3, &buf[ii]) != (unsigned long)((buf[ii + len + 3] << 16) |
                                                    (buf[ii + len + 4] << 8) | buf[ii + len + 5])) {
      ii++;
      continue;
    }
    // exact-size copy, so reading beyond the frame is caught as well
    vector<unsigned char> frame(buf.begin() + ii, buf.begin() + ii + len + 6);
    int type = (frame[3] << 4) | (frame[4] >> 4);
    printf("frame at %zu: type %d, %zu bytes\n", ii, type, len);
    decode("IGS", &igs, frame.data(), frame.size());
    decode("RTCM", &rtcm, frame.data(), frame.size());
    decode("RTCMnew", &rtcmNew, frame.data(), frame.size());
    ii += len + 6;
  }
  if (nFailed) {
    printf("%d decoded messages differ after re-encoding\n", nFailed);
    return 1;
  }
  return 0;
}