#include "GPS_receiver.hh"

#include "aux.hh"

#include "global_constants.hh"
#include "matrix/utility.hh"
#include "stochastic.hh"

#include "cad_utility.hh"

#include <tuple>

#include <armadillo>

#include "Earth.hh"
#include "Euler.hh"
#include "GPS_satellites.hh"
#include "Ins.hh"
#include "Newton.hh"

GPS_Receiver::GPS_Receiver(Newton &ntn, _Euler_ &elr)
    : newton(&ntn),
      euler(&elr),
      MATRIX_INIT(PP, 8, 8),
      MATRIX_INIT(FF, 8, 8),
      MATRIX_INIT(PHI, 8, 8),
      VECTOR_INIT(SXH, 3),
      VECTOR_INIT(VXH, 3),
      VECTOR_INIT(CXH, 3) {
  this->default_data();
}

GPS_Receiver::GPS_Receiver(const GPS_Receiver &other)
    : newton(other.newton),
      euler(other.euler),
      gps_sats(other.gps_sats),
      MATRIX_INIT(PP, 8, 8),
      MATRIX_INIT(FF, 8, 8),
      MATRIX_INIT(PHI, 8, 8),
      VECTOR_INIT(SXH, 3),
      VECTOR_INIT(VXH, 3),
      VECTOR_INIT(CXH, 3) {
  this->default_data();

  /* Input File */
  memcpy(this->slot, other.slot, sizeof(this->slot));

  /* GPS Device parameters */
  this->del_rearth = other.del_rearth;
  this->gps_acqtime = other.gps_acqtime;
  this->gps_step = other.gps_step;

  this->ucfreq_noise = other.ucfreq_noise;
  this->ucfreq_noise_sigma = other.ucfreq_noise_sigma;
  this->ucfreq_noise_bcor = other.ucfreq_noise_bcor;
  this->ucbias_error = other.ucbias_error;

  memcpy(this->PR_BIAS, other.PR_BIAS, sizeof(this->PR_BIAS));
  memcpy(this->PR_NOISE, other.PR_NOISE, sizeof(this->PR_NOISE));
  memcpy(this->PR_NOISE_sigma, other.PR_NOISE_sigma,
         sizeof(this->PR_NOISE_sigma));
  memcpy(this->PR_NOISE_bcor, other.PR_NOISE_bcor, sizeof(this->PR_NOISE_bcor));
  memcpy(this->DR_NOISE, other.DR_NOISE, sizeof(this->DR_NOISE));
  memcpy(this->DR_NOISE_sigma, other.DR_NOISE_sigma,
         sizeof(this->DR_NOISE_sigma));
  memcpy(this->DR_NOISE_bcor, other.DR_NOISE_bcor, sizeof(this->DR_NOISE_bcor));

  /* GPS EKF Parameters */
  this->ppos = other.ppos;
  this->pvel = other.pvel;
  this->qpos = other.qpos;
  this->qvel = other.qvel;
  this->rpos = other.rpos;
  this->rvel = other.rvel;
  this->factr = other.factr;

  /* Internal variables */
  this->gps_acq = this->gps_acq;
  this->gps_epoch = other.gps_epoch;
  this->time_gps = other.time_gps;

  this->FF = other.FF;
  this->PHI = other.PHI;
  this->PP = other.PP;

  this->factq = other.factq;
  this->qclockb = other.qclockb;
  this->qclockf = other.qclockf;

  this->slotsum = other.slotsum;

  this->SXH = other.SXH;
  this->VXH = other.VXH;
  this->CXH = other.CXH;

  /* GPS Outputs */
  this->gdop = other.gdop;
  memcpy(this->ssii_quad, other.ssii_quad, sizeof(this->ssii_quad));
  memcpy(this->vsii_quad, other.vsii_quad, sizeof(this->vsii_quad));

  this->ucfreq_error = other.ucfreq_error;
  this->ucfreqm = other.ucfreqm;

  this->std_pos = other.std_pos;
  this->std_vel = other.std_vel;
  this->std_ucbias = other.std_ucbias;

  this->lat1 = other.lat1;
  this->lat2 = other.lat2;
  this->lat3 = other.lat3;
  this->lat4 = other.lat4;
  this->lon1 = other.lon1;
  this->lon2 = other.lon2;
  this->lon3 = other.lon3;
  this->lon4 = other.lon4;
  this->alt1 = other.alt1;
  this->alt2 = other.alt2;
  this->alt3 = other.alt3;
  this->alt4 = other.alt4;

  this->gps_pos_meas = other.gps_pos_meas;
  this->gps_vel_meas = other.gps_vel_meas;
  this->state_pos = other.state_pos;
  this->state_vel = other.state_vel;
}

GPS_Receiver &GPS_Receiver::operator=(const GPS_Receiver &other) {
  if (&other == this) return *this;

  this->newton = other.newton;
  this->euler = other.euler;
  this->gps_sats = other.gps_sats;

  /* Input File */
  memcpy(this->slot, other.slot, sizeof(this->slot));

  /* GPS Device parameters */
  this->del_rearth = other.del_rearth;
  this->gps_acqtime = other.gps_acqtime;
  this->gps_step = other.gps_step;

  this->ucfreq_noise = other.ucfreq_noise;
  this->ucfreq_noise_sigma = other.ucfreq_noise_sigma;
  this->ucfreq_noise_bcor = other.ucfreq_noise_bcor;
  this->ucbias_error = other.ucbias_error;

  memcpy(this->PR_BIAS, other.PR_BIAS, sizeof(this->PR_BIAS));
  memcpy(this->PR_NOISE, other.PR_NOISE, sizeof(this->PR_NOISE));
  memcpy(this->PR_NOISE_sigma, other.PR_NOISE_sigma,
         sizeof(this->PR_NOISE_sigma));
  memcpy(this->PR_NOISE_bcor, other.PR_NOISE_bcor, sizeof(this->PR_NOISE_bcor));
  memcpy(this->DR_NOISE, other.DR_NOISE, sizeof(this->DR_NOISE));
  memcpy(this->DR_NOISE_sigma, other.DR_NOISE_sigma,
         sizeof(this->DR_NOISE_sigma));
  memcpy(this->DR_NOISE_bcor, other.DR_NOISE_bcor, sizeof(this->DR_NOISE_bcor));

  /* GPS EKF Parameters */
  this->ppos = other.ppos;
  this->pvel = other.pvel;
  this->qpos = other.qpos;
  this->qvel = other.qvel;
  this->rpos = other.rpos;
  this->rvel = other.rvel;
  this->factr = other.factr;

  /* Internal variables */
  this->gps_acq = this->gps_acq;
  this->gps_epoch = other.gps_epoch;
  this->time_gps = other.time_gps;

  this->FF = other.FF;
  this->PHI = other.PHI;
  this->PP = other.PP;

  this->factq = other.factq;
  this->qclockb = other.qclockb;
  this->qclockf = other.qclockf;

  this->slotsum = other.slotsum;

  this->SXH = other.SXH;
  this->VXH = other.VXH;
  this->CXH = other.CXH;

  /* GPS Outputs */
  this->gdop = other.gdop;
  memcpy(this->ssii_quad, other.ssii_quad, sizeof(this->ssii_quad));
  memcpy(this->vsii_quad, other.vsii_quad, sizeof(this->vsii_quad));

  this->ucfreq_error = other.ucfreq_error;
  this->ucfreqm = other.ucfreqm;

  this->std_pos = other.std_pos;
  this->std_vel = other.std_vel;
  this->std_ucbias = other.std_ucbias;

  this->lat1 = other.lat1;
  this->lat2 = other.lat2;
  this->lat3 = other.lat3;
  this->lat4 = other.lat4;
  this->lon1 = other.lon1;
  this->lon2 = other.lon2;
  this->lon3 = other.lon3;
  this->lon4 = other.lon4;
  this->alt1 = other.alt1;
  this->alt2 = other.alt2;
  this->alt3 = other.alt3;
  this->alt4 = other.alt4;

  this->gps_pos_meas = other.gps_pos_meas;
  this->gps_vel_meas = other.gps_vel_meas;
  this->state_pos = other.state_pos;
  this->state_vel = other.state_vel;

  return *this;
}

void GPS_Receiver::default_data() {
  gps_update = 0;

  // setting inital acquisition flag
  gps_acq = false;
}

void GPS_Receiver::setup_state_covariance_matrix(double factp, double pclockb,
                                                 double pclockf) {
  PP = arma::mat88(arma::fill::zeros);
  for (int i = 0; i < 3; i++) {
    PP(i, i) = pow(ppos * (1 + factp), 2);
    PP(i + 3, i + 3) = pow(pvel * (1 + factp), 2);
  }
  PP(6, 6) = pow(pclockb * (1 + factp), 2);
  PP(7, 7) = pow(pclockf * (1 + factp), 2);

  return;
}

void GPS_Receiver::setup_error_covariance_matrix(double factq, double qclockb,
                                                 double qclockf) {
  this->factq = factq;
  this->qclockb = qclockb;
  this->qclockf = qclockf;

  return;
}

void GPS_Receiver::setup_fundamental_dynamic_matrix(double uctime_cor) {
  // fundamental dynamic matrix of filter - constant throughout
  FF = arma::mat88(arma::fill::zeros);
  FF(0, 3) = 1;
  FF(1, 4) = 1;
  FF(2, 5) = 1;
  FF(6, 7) = 1;
  FF(7, 7) = -1 / uctime_cor;

  return;
}

void GPS_Receiver::initialize(GPS_Satellites *sats, double int_step) {
  gps_sats = sats;

  // state transition matrix - constant throughout
  PHI = arma::mat88(arma::fill::eye) + FF * int_step +
        FF * FF * (int_step * int_step / 2);

  // initializing update clock
  gps_epoch = get_elapsed_time();
}

void GPS_Receiver::update_markov(double int_step) {
  int i;

  ucfreq_noise = markov(ucfreq_noise_sigma, ucfreq_noise_bcor,
                        get_elapsed_time(), int_step, ucfreq_noise);
  for (i = 0; i < 4; i++) {
    PR_NOISE[i] = markov(PR_NOISE_sigma[i], PR_NOISE_bcor[i],
                         get_elapsed_time(), int_step, PR_NOISE[i]);
  }
  for (i = 0; i < 4; i++) {
    DR_NOISE[i] = markov(DR_NOISE_sigma[i], DR_NOISE_bcor[i],
                         get_elapsed_time(), int_step, DR_NOISE[i]);
  }
}

void GPS_Receiver::get_quadriga() {
  int i(0);
  int j(0);
  int m(0);

  gdop = LARGE;
  int quad[4] = {0, 0, 0, 0};  // location of quadriga SVs
                               // in 'ssii_vis[visible_count]

  // conversion to inertial (J2000) coordinates
  double ssii[24][4];
  double sin_incl = sin(gps_sats->inclination);
  double cos_incl = cos(gps_sats->inclination);
  for (i = 0; i < 24; i++) {
    ssii[i][0] = gps_sats->radius *
                 (cos(gps_sats->sv_data[i][0]) * cos(gps_sats->sv_data[i][1]) -
                  sin(gps_sats->sv_data[i][0]) * sin(gps_sats->sv_data[i][1]) *
                      cos_incl);
    ssii[i][1] = gps_sats->radius *
                 (sin(gps_sats->sv_data[i][0]) * cos(gps_sats->sv_data[i][1]) +
                  cos(gps_sats->sv_data[i][0]) * sin(gps_sats->sv_data[i][1]) *
                      cos_incl);
    ssii[i][2] = gps_sats->radius * sin(gps_sats->sv_data[i][1]) * sin_incl;
    ssii[i][3] = 0;
    // last entry is a flag with the code:
    // =0:not visible; >0:visible (not int but double!), where the
    // number
    // is the SV slot# (1,2,3,...,24)
  }

  // determining visible satellites
  arma::vec3 SSII;
  arma::vec3 SBII = newton->get_SBII();
  int visible_count(0);

  for (i = 0; i < 24; i++) {
    SSII[0] = ssii[i][0];
    SSII[1] = ssii[i][1];
    SSII[2] = ssii[i][2];

    // SV to user angle with vertex at Earth center
    double delta = angle(SSII, SBII);

    // grazing angle of SV beam with vertex at Earth center
    double epsilon = acos((Earth::radius + del_rearth) / gps_sats->radius);

    // min radius of user to have clear line-of-site to SV
    double rmin = (Earth::radius + del_rearth) / cos(delta - epsilon);

    if (delta < epsilon) {
      // SV is in the visibility cone
      ssii[i][3] = i + 1;
      visible_count++;
    } else {
      // user is outside the visibility cone but high enough to have
      // LOS to the SV
      // (rmin can go negative if delta-epsilon>90deg; this can happen
      // when the user is opposite (or nearly opposite) of the SV;
      // this is always a no-visibility case)
      double dbi = norm(SBII);
      if (rmin > 0 && rmin < dbi) {
        ssii[i][3] = i + 1;
        visible_count++;
      }
    }
  }
  if (visible_count <
      4) {  // re-acquiring GPS if not enough SVs visible (less than 4)
    gps_epoch = get_elapsed_time();
    gps_acq = false;
  } else {  // selecting best 4 SVs if 4 or more are visible
    // repackage visible SVs into 'ssii_vis' single-dimensioned array
    // inertial displacement vector elements are stored sequentially in
    // 'ssii_vis[4*visible_count]'
    // 'ssii_vis' has 3 inertial coordinates and SV slot# of all visible
    // SVs
    double *ssii_vis;
    ssii_vis = new double[4 * visible_count];
    int k(0);
    for (i = 0; i < 24; i++) {
      if (ssii[i][3] > 0) {
        *(ssii_vis + k) = ssii[i][0];
        *(ssii_vis + k + 1) = ssii[i][1];
        *(ssii_vis + k + 2) = ssii[i][2];
        *(ssii_vis + k + 3) = ssii[i][3];
        k = k + 4;
      }
    }
    // selecting quadriga (four SVs) with smallest GDOP
    // i1, i2, i3, i4 are the SVs picked by the binomial combination
    int nm3 = visible_count - 3;  // nm3=1
    int nm2 = visible_count - 2;  // nm2=2
    int nm1 = visible_count - 1;  // nm1=3

    for (int i1 = 0; i1 < nm3; i1++) {
      for (int i2 = i1 + 1; i2 < nm2; i2++) {
        for (int i3 = i2 + 1; i3 < nm1; i3++) {
          for (int i4 = i3 + 1; i4 < visible_count; i4++) {
            // pullling the quadriga inertial coordinates
            arma::vec3 SSII1;
            arma::vec3 SSII2;
            arma::vec3 SSII3;
            arma::vec3 SSII4;
            for (m = 0; m < 3; m++) {
              SSII1[m] = *(ssii_vis + 4 * i1 + m);
              SSII2[m] = *(ssii_vis + 4 * i2 + m);
              SSII3[m] = *(ssii_vis + 4 * i3 + m);
              SSII4[m] = *(ssii_vis + 4 * i4 + m);
            }
            // calculating user wrt the SV displacement unit
            // vectors
            arma::vec UNI1 = normalise(SBII - SSII1);
            arma::vec UNI2 = normalise(SBII - SSII2);
            arma::vec UNI3 = normalise(SBII - SSII3);
            arma::vec UNI4 = normalise(SBII - SSII4);

            // building the GPS 'H' matrix
            arma::mat44 HGPS(arma::fill::ones);
            HGPS.col(0) = UNI1;
            HGPS.col(1) = UNI2;
            HGPS.col(2) = UNI3;
            HGPS.col(3) = UNI4;
            // calculating GDOP
            arma::mat44 COV;
            COV = arma::inv(HGPS * arma::trans(HGPS));
            double gdop_local = sqrt(sum(COV.diag()));

            // save slot # of quadriga SVs if GDOP has decreased
            if (gdop_local < gdop) {
              gdop = gdop_local;
              quad[0] = i1;
              quad[1] = i2;
              quad[2] = i3;
              quad[3] = i4;
            }
          }
        }
      }
    }  // end of picking quadriga amongst visible SVs

    // extracting "best" quadriga from visible SVs
    // and storing inertial coordinates of the four SVs and their slot#
    // in ssii_quad[16]
    for (m = 0; m < 4; m++) {
      for (int n = 0; n < 4; n++) {
        *(ssii_quad + 4 * m + n) = *(ssii_vis + 4 * quad[m] + n);
      }
    }
    delete[] ssii_vis;

    // calculating inertial velocity of quadriga SVs
    // getting slot# of quadriga
    //
    int *islot = new int[i];
    for (i = 0; i < 4; i++) {
      slot[i] = *(ssii_quad + 4 * i + 3);
      // casting into an int
      islot[i] = static_cast<int> slot[i];
    }
    // storing inertial velocities of the four SVs in vsii_quad[12]
    double sin_incl = sin(gps_sats->inclination);
    double cos_incl = cos(gps_sats->inclination);
    double vel = gps_sats->radius * gps_sats->angular_velocity;
    for (m = 0; m < 4; m++) {
      int ii = islot[m] - 1;  // reminder: slot#=1,2,3...,24
      *(vsii_quad + 3 * m + 0) =
          vel *
          (-sin(gps_sats->sv_data[ii][1]) * cos(gps_sats->sv_data[ii][0]) -
           cos(gps_sats->sv_data[ii][1]) * sin(gps_sats->sv_data[ii][0]) *
               cos_incl);
      *(vsii_quad + 3 * m + 1) =
          vel *
          (-sin(gps_sats->sv_data[ii][1]) * sin(gps_sats->sv_data[ii][0]) +
           cos(gps_sats->sv_data[ii][1]) * cos(gps_sats->sv_data[ii][0]) *
               cos_incl);
      *(vsii_quad + 3 * m + 2) =
          vel * (cos(gps_sats->sv_data[ii][1]) * sin_incl);
    }
  }  // end of picking quadriga from 4 or more visible SVs
}

void GPS_Receiver::filter_extrapolation(double int_step) {
  arma::mat88 QQ(arma::fill::zeros);  // local

  // *** user-clock frequency and bias error growth between updates ***
  // integrating 'ucfreq_noise' Markov process to
  // obtain user-clock bias error 'ucbias_error' (trapezoidal
  // integration)
  // user-clock bias is updated at filter update epoch
  ucfreq_error = ucfreq_noise;
  ucbias_error = ucbias_error + (ucfreq_error + ucfreqm) * (int_step / 2);
  ucfreqm = ucfreq_error;

  // *** filter extrapolation ***
  // dynamic error covariance matrix
  for (int i = 0; i < 3; i++) {
    QQ(i, i) = pow(qpos * (1 + factq), 2);
    QQ(i + 3, i + 3) = pow(qvel * (1 + factq), 2);
  }
  QQ(6, 6) = pow(qclockb * (1 + factq), 2);
  QQ(7, 7) = pow(qclockf * (1 + factq), 2);

  // covariance estimate extrapolation
  PP = PHI * (PP + QQ * (int_step / 2)) * trans(PHI) + QQ * (int_step / 2);

  // diagnostics: st. deviations of the diagonals of the covariance matrix
  std_pos = sqrt(PP(0, 0));
  std_vel = sqrt(PP(3, 3));
  std_ucbias = sqrt(PP(6, 6));
}

void GPS_Receiver::measure() {
  double dtime_gps;
  /* Testing GPS timing for update and acquire */
  if (!gps_acq)
    // saving delay-time for GPS signal acquisition
    dtime_gps = gps_acqtime;
  else
    // saving delay-time for GPS update
    dtime_gps = gps_step;
  // checking when GPS update time has occured in order to initiate update
  time_gps = get_elapsed_time() - gps_epoch;
  if (time_gps < dtime_gps) {
    return;
  }

  gps_acq = true;
  gps_update++;
  // resetting update clock
  time_gps = 0;
  gps_epoch = get_elapsed_time();

  /* GPS Update and Measurement */
  double slotm(0);

  arma::vec8 ZZ(arma::fill::zeros);
  arma::vec8 XH(arma::fill::zeros);   // local
  arma::mat88 RR(arma::fill::zeros);  // local
  arma::mat88 HH(arma::fill::zeros);  // local

  // ***
  // SV propagation and quadriga selection 'ssii_quad'
  // (4 SVs with best GDOP)
  // ***
  // gps_quadriga(ssii_quad,vsii_quad,gdop,mgps,
  // sv_init_data,rsi,wsi,incl,almanac_time,del_rearth,time,SBII);

  // Pseudo-range and range-rate measurements
  for (int i = 0; i < 4; i++) {
    // unpacking i-th SV inertial position
    arma::vec3 SSII;
    for (int j = 0; j < 3; j++) {
      SSII[j] = *(ssii_quad + 4 * i + j);
    }
    // Z150126 - start
    // diagnostics: getting long, lat, alt of the four quadriga SVs for
    // plotting in GLOBE
    double lon(0), lat(0), alt(0);
    std::tie(lon, lat, alt) = cad::geo84_in(SSII, gps_sats->time);
    switch (i) {
      case 0:
        lon1 = lon * DEG;
        lat1 = lat * DEG;
        alt1 = alt;
        break;
      case 1:
        lon2 = lon * DEG;
        lat2 = lat * DEG;
        alt2 = alt;
        break;
      case 2:
        lon3 = lon * DEG;
        lat3 = lat * DEG;
        alt3 = alt;
        break;
      case 3:
        lon4 = lon * DEG;
        lat4 = lat * DEG;
        alt4 = alt;
        break;
    }

    arma::vec3 SBII = newton->get_SBII();
    arma::vec3 VBII = newton->get_VBII();
    arma::vec3 WBII = euler->get_WBII();

    arma::vec3 SBIIC = grab_SBIIC();
    arma::vec3 VBIIC = grab_VBIIC();
    arma::vec3 WBICI = grab_WBICI();

    // calculating true range to SV
    arma::vec3 SSBI;

    SSBI = SSII - SBII;
    double dsb = norm(SSBI);

    // measured pseudo-range
    double dsb_meas = dsb + PR_BIAS[i] + PR_NOISE[i] + ucbias_error;

    // unpacking i-th SV inertial velocity
    arma::vec3 VSII;
    for (int j = 0; j < 3; j++) {
      VSII[j] = *(vsii_quad + 3 * i + j);
    }
    // velocity of SV wrt user
    arma::vec3 VSBI;
    VSBI = VSII - VBII - skew_sym(WBII) * SSBI;

    // calculating true range-rate to SV
    arma::vec3 USSBI;
    USSBI = SSBI * (1 / dsb);
    double dvsb = dot(VSBI, USSBI);

    // measured delta-range rate
    double dvsb_meas = dvsb + DR_NOISE[i] + ucfreq_error;

    // INS derived range measurements
    arma::vec3 SSBIC;
    SSBIC = SSII - SBIIC;
    double dsbc = norm(SSBIC);

    // INS derived range-rate measurements
    // velocity of SV wrt user
    arma::vec3 VSBIC;
    VSBIC = VSII - VBIIC - skew_sym(WBICI) * SSBIC;
    // calculating range-rate to SV
    arma::vec3 USSBIC;
    USSBIC = SSBIC * (1 / dsb);
    double dvsbc = dot(VSBIC, USSBIC);

    // loading measurement residuals into measurement vector
    // ZZ[0->3] range meas resid of SV's;
    // ZZ[4->7] range-rate meas resid of SV's
    ZZ[i] = dsb_meas - dsbc;
    ZZ[i + 4] = dvsb_meas - dvsbc;

    // observation matrix of filter
    for (int j = 0; j < 3; j++) {
      HH(i, j) = USSBI(j, 0);
      HH(i + 4, j + 3) = USSBI(j, 0) * gps_step;
    }
    HH(i, 6) = 1;
    HH(i + 4, 7) = gps_step;

    // for diagnostics: loading the 4 SV slot # of the quadriga
    *(slot + i) = *(ssii_quad + 4 * i + 3);
    // accumulating sum of slots
    slotm = slotm + slot[i];
  }
  // for diagnostics displaying the SV slot# of the quadriga on the
  // console
  // but only if they have changed (i.e., sum of slot# has changed)

  if (slotsum != slotm) {
    slotsum = slotm;
    std::cout << " *** GPS Quadriga slot # " << slot[0] << "  " << slot[1]
              << "  " << slot[2] << "  " << slot[3] << " ;  GDOP = " << gdop
              << " m ***\n";
  }
  // *** filter correction and update (to INS: 'SXH' and 'VXH') ***
  // filter gain
  arma::mat88 KK(arma::fill::zeros);

  // measurement noise covariance matrix
  for (int i = 0; i < 4; i++) {
    RR(i, i) = pow(rpos * (1 + factr), 2);
    RR(i + 4, i + 4) = pow(rvel * (1 + factr), 2);
  }
  // Kalman gain
  KK = inv(PP * trans(HH) * (HH * PP * trans(HH) + RR));
  // state correction
  XH = KK * ZZ;
  // covariance correction for next cycle
  PP = (arma::mat88(arma::fill::eye) - KK * HH) * PP;

  // clock error bias update
  ucbias_error = ucbias_error - XH(6, 0);

  // diagnostics of 1st SV of quadriga saved to plot file
  gps_pos_meas = ZZ(0, 0);
  gps_vel_meas = ZZ(4, 0);

  // decomposing state vector for output
  for (int m = 0; m < 3; m++) {
    SXH(m, 0) = XH(m, 0);
    VXH(m, 0) = XH(m + 3, 0);
  }
  CXH(0, 0) = XH(6, 0);
  CXH(1, 0) = XH(7, 0);

  // diagnostic
  state_pos = norm(SXH);
  state_vel = norm(VXH);
}

arma::vec3 GPS_Receiver::get_SXH() { return SXH; }
arma::vec3 GPS_Receiver::get_VXH() { return VXH; }
arma::vec3 GPS_Receiver::get_CXH() { return CXH; }


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Alexander Sokolov <asokolov@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------////

#ifndef CRYPTO3_MERKLE_DAMGARD_PADDING_HPP
#define CRYPTO3_MERKLE_DAMGARD_PADDING_HPP

#include <boost/crypto3/detail/inject.hpp>
#include <boost/crypto3/detail/pack.hpp>
#include <boost/crypto3/detail/octet.hpp>

namespace boost {
    namespace crypto3 {
        namespace hashes {
            namespace detail {
                template<typename Hash>
                class merkle_damgard_padding {
                    typedef Hash policy_type;

                    typedef typename policy_type::digest_endian endian_type;

                    constexpr static const std::size_t word_bits = policy_type::word_bits;
                    typedef typename policy_type::word_type word_type;

                    constexpr static const std::size_t state_bits = policy_type::state_bits;
                    constexpr static const std::size_t state_words = policy_type::state_words;
                    typedef typename policy_type::state_type state_type;

                    constexpr static const std::size_t block_bits = policy_type::block_bits;
                    constexpr static const std::size_t block_words = policy_type::block_words;
                    typedef typename policy_type::block_type block_type;

                    constexpr static const std::size_t digest_bits = policy_type::digest_bits;
                    typedef typename policy_type::digest_type digest_type;

                    typedef ::boost::crypto3::detail::injector<endian_type, word_bits, block_words, block_bits>
                        injector_type;

                public:
                    void operator()(block_type &block, std::size_t &block_seen) {
                        using namespace boost::crypto3::detail;
                        // Remove garbage
                        block_type block_of_zeros;
                        std::size_t seen_copy = block_seen;
                        std::fill(block_of_zeros.begin(), block_of_zeros.end(), 0);
                        injector_type::inject(block_of_zeros, block_bits - block_seen, block, seen_copy);

                        // Get bit 1 in the endianness used by the hashes
                        std::array<octet_type, word_bits / octet_bits> bit_one = {{0x80}};
                        std::array<word_type, 1> bit_one_word {};
                        pack<stream_endian::big_octet_big_bit, endian_type, octet_bits, word_bits>(
                            bit_one.begin(), bit_one.end(), bit_one_word.begin());

                        // Add 1 bit to block
                        injector_type::inject(bit_one_word[0], 1, block, block_seen);
                    }
                };
            }    // namespace detail
        }        // namespace hashes
    }            // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_MERKLE_DAMGARD_PADDING_HPP


#include <algorithm>
#include <cfloat>
#include <vector>
#include <boost/pointer_cast.hpp>
#include <boost/shared_ptr.hpp>

#include "caffe/layers/weighted_contrastive_loss_layer.hpp"
#include "caffe/util/math_functions.hpp"

namespace caffe {

template <typename Dtype>
void WeightedContrastiveLossLayer<Dtype>::LayerSetUp(
    const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {

  LossLayer<Dtype>::LayerSetUp(bottom, top);
  //auxiliary layer
  LayerParameter contrastive_loss_param(this->layer_param_);
  contrastive_loss_param.set_type("ContrastiveLoss");
  contrastive_loss_layer_ = LayerRegistry<Dtype>::CreateLayer(contrastive_loss_param);

}

template <typename Dtype>
void WeightedContrastiveLossLayer<Dtype>::Reshape(
    const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {

  LossLayer<Dtype>::Reshape(bottom, top);
  //initialization of  pair data and pair lables, and auxiliary parameters
  sample_num = bottom[0]->num();
  feature_dim = bottom[0]->channels();
  //
  pair_num = 0;
  pair_index_a.clear();
  pair_index_b.clear();
  pair_score.clear();
  for (int i = 0; i < sample_num; i++){
    for(int j = 0; j < sample_num; j++){
      if(j != i){
        pair_num++;
        pair_index_a.push_back(i);
        pair_index_b.push_back(j);
        pair_score.push_back(Dtype(1.0));
      }
    }
  }
  //
  vector<int> shape = bottom[0]->shape();
  shape[0] = pair_num;
  pair_data_a.Reshape(shape);
  pair_data_b.Reshape(shape);
  pair_label.Reshape(pair_num, 1, 1, 1);

  contrastive_loss_bottom.clear();
  contrastive_loss_bottom.push_back(&pair_data_a);
  contrastive_loss_bottom.push_back(&pair_data_b);
  contrastive_loss_bottom.push_back(&pair_label);

  contrastive_loss_top.clear();
  contrastive_loss_top.push_back(top[0]);

  contrastive_loss_layer_->SetUp(contrastive_loss_bottom, contrastive_loss_top);

  contrastive_propogate.clear();
  contrastive_propogate.push_back(true);
  contrastive_propogate.push_back(true);
  contrastive_propogate.push_back(false);

}

template <typename Dtype>
void WeightedContrastiveLossLayer<Dtype>::Forward_cpu(
    const vector<Blob<Dtype>*>& bottom, const vector<Blob<Dtype>*>& top) {

  //set the input data
  for (int i = 0; i < pair_num; i++){
    //The pair label
    if( bottom[1]->cpu_data()[pair_index_a[i]] == bottom[1]->cpu_data()[pair_index_b[i]] ){
      pair_label.mutable_cpu_data()[i] = Dtype(1);
    }
    else{
      pair_label.mutable_cpu_data()[i] = Dtype(0);
    }
    //the pair data
    const Dtype* source_a = bottom[0]->cpu_data() + pair_index_a[i] * feature_dim;
    const Dtype* source_b = bottom[0]->cpu_data() + pair_index_b[i] * feature_dim;
    Dtype* dest_a = pair_data_a.mutable_cpu_data() + i*feature_dim;
    Dtype* dest_b = pair_data_b.mutable_cpu_data() + i*feature_dim;

    caffe_copy(feature_dim, source_a, dest_a);
    caffe_copy(feature_dim, source_b, dest_b);
    //the pair score
    //pair_score[i] = bottom[2]->cpu_data()[pair_index_a[i]] * bottom[2]->cpu_data()[pair_index_b[i]];
    pair_score[i] = std::min(bottom[2]->cpu_data()[pair_index_a[i]] , bottom[2]->cpu_data()[pair_index_b[i]]);
  }
  // The forward pass of contrastive loss.
  contrastive_loss_layer_->Forward(contrastive_loss_bottom, contrastive_loss_top);
}

template <typename Dtype>
void WeightedContrastiveLossLayer<Dtype>::Backward_cpu(const vector<Blob<Dtype>*>& top,
    const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom) {

  if (propagate_down[0]) {
    // The backward pass of contrastive loss.
    contrastive_loss_layer_->Backward(contrastive_loss_top, contrastive_propogate, contrastive_loss_bottom);
    // set diff to zero
    caffe_set(bottom[0]->count(), Dtype(0), bottom[0]->mutable_cpu_diff());

    //dynamic cast
    shared_ptr<ContrastiveLossLayer<Dtype> > auxiLayer = boost::dynamic_pointer_cast<ContrastiveLossLayer<Dtype> >(contrastive_loss_layer_);
    if (!auxiLayer) {
    	throw std::runtime_error("ContrastiveLossLayer dynamic pointer_cast failed");
  	}

    //Hard Mining of valid pairs and valid pair scores
    hardest_index_vec.clear();
    sum_score = Dtype(0);
    //
    int pair_index = -1;
    for (int i = 0; i < sample_num; i++){
    	Dtype positive_max_distance = Dtype(0);
    	//Dtype negative_min_distance = Dtype(2);
    	Dtype negative_min_distance = Dtype(4);
    	int positive_pair_index = -1;
    	int negative_pair_index = -1;
    	for (int j = 0; j < sample_num; j++){
    		if(j != i){
	          pair_index++;

	          if( static_cast<int>(pair_label.cpu_data()[pair_index]) ){
	            //process similiar pairs: positives
	            if(auxiLayer->dist_sq_.cpu_data()[pair_index] > positive_max_distance){
	              positive_max_distance = auxiLayer->dist_sq_.cpu_data()[pair_index];
	              positive_pair_index = pair_index;
	            }
	          }
	          else{
	            //process dissimiliar pairs: negatives
	            if(auxiLayer->dist_sq_.cpu_data()[pair_index] < negative_min_distance){
	              negative_min_distance = auxiLayer->dist_sq_.cpu_data()[pair_index];
	              negative_pair_index = pair_index;
	            }
	          }
	        }
    	}
    	if (positive_pair_index != -1){
	        hardest_index_vec.push_back(positive_pair_index);

	        sum_score += pair_score[positive_pair_index];
    	}
    	if (negative_pair_index != -1){
	        hardest_index_vec.push_back(negative_pair_index);

	        sum_score += pair_score[negative_pair_index];
    	}
    }
    sum_score += static_cast<Dtype>(1e-12);

    //copy gradients and scale the gradients with normalized score
    for (int valid_ind = 0; valid_ind < hardest_index_vec.size(); valid_ind++ )
    {
      //i-th pair
      int i = hardest_index_vec[valid_ind];
      pair_score[i] = pair_score[i] / sum_score;
      //LOG(INFO) << "pair_score[" << i <<"]: " << pair_score[i];

      const Dtype* source_a = pair_data_a.cpu_diff() + i * feature_dim;
      const Dtype* source_b = pair_data_b.cpu_diff() + i * feature_dim;
      Dtype* dest_a = bottom[0]->mutable_cpu_diff() + pair_index_a[i] * feature_dim;
      Dtype* dest_b = bottom[0]->mutable_cpu_diff() + pair_index_b[i] * feature_dim;
      //dest[ind] = pair_score[i] * source[ind] + dest[ind]
      caffe_axpy(feature_dim, pair_score[i], source_a, dest_a);
      caffe_axpy(feature_dim, pair_score[i], source_b, dest_b);
    }

  }
  if (propagate_down[1] || propagate_down[2]) {
    LOG(FATAL) << this->type()
               << " Layer cannot backpropagate to label and score inputs.";
  }

}

#ifdef CPU_ONLY
STUB_GPU(WeightedContrastiveLossLayer);
#endif

INSTANTIATE_CLASS(WeightedContrastiveLossLayer);
REGISTER_LAYER_CLASS(WeightedContrastiveLoss);

}  // namespace caffe


#ifndef BOOST_METAPARSE_V1_UTIL_IN_RANGE_HPP
#define BOOST_METAPARSE_V1_UTIL_IN_RANGE_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2009 - 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <boost/mpl/less_equal.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/quote.hpp>
#include <boost/mpl/bool.hpp>

#include <boost/mpl/vector.hpp>

namespace boost
{
  namespace metaparse
  {
    namespace v1
    {
      namespace util
      {
        template <
          class LowerBound = boost::mpl::na,
          class UpperBound = boost::mpl::na,
          class Item = boost::mpl::na
        >
        struct in_range :
          boost::mpl::bool_<
            boost::mpl::less_equal<LowerBound, Item>::type::value
            && boost::mpl::less_equal<Item, UpperBound>::type::value
          >
        {};

        template <class LowerBound, class UpperBound>
        struct in_range<LowerBound, UpperBound, boost::mpl::na>
        {
          typedef in_range type;

          template <class Item = boost::mpl::na>
          struct apply : in_range<LowerBound, UpperBound, Item> {};
        };

        template <class LowerBound>
        struct in_range<LowerBound, boost::mpl::na, boost::mpl::na>
        {
          typedef in_range type;

          template <
            class UpperBound = boost::mpl::na,
            class Item = boost::mpl::na
          >
          struct apply : in_range<LowerBound, UpperBound, Item> {};
        };

        template <>
        struct in_range<boost::mpl::na, boost::mpl::na, boost::mpl::na>
        {
          typedef in_range type;

          template <
            class LowerBound = boost::mpl::na,
            class UpperBound = boost::mpl::na,
            class Item = boost::mpl::na
          >
          struct apply : in_range<LowerBound, UpperBound, Item> {};
        };
      }
    }
  }
}

#endif


// Copyright (c) 2015-2016, Kai Wolf
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include "io/dataset_reader.hpp"

#include <algorithm>
#include <cassert>
#include <iterator>
#include <ostream>
#include <type_traits>
#include <boost/filesystem.hpp>
#include <boost/format.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "common/camera.hpp"
#include "common/dataset.hpp"

namespace fs = boost::filesystem;
namespace ret {

namespace io {

    DataSetReader::DataSetReader(std::string directory)
        : directory_(std::move(directory)) {}

    std::shared_ptr<DataSet> DataSetReader::load(
        const std::size_t numImages) const {

        cv::Mat K, dist;
        if (fs::exists(directory_ + "/K.xml")) {
            K = loadMatrixFromFile("/K.xml", "K_matrix");
        }

        if (fs::exists(directory_ + "/dist.xml")) {
            dist = loadMatrixFromFile("/dist.xml", "dist_coeff");
        }
        auto projMats = loadProjectionMatrices(numImages, "/viff.xml");

        fs::path dir(directory_);
        auto ds = std::make_shared<DataSet>();
        if (fs::exists(dir) && fs::is_directory(dir)) {
            // sorting paths, since directory listing is not sorted under Linux
            using paths = std::vector<fs::path>;
            paths stored_paths;
            std::copy(fs::directory_iterator(dir), fs::directory_iterator(),
                      std::back_inserter(stored_paths));
            std::sort(stored_paths.begin(), stored_paths.end());

            std::size_t camIdx = 0;
            for (paths::const_iterator iter(stored_paths.begin());
                 iter != stored_paths.end(); ++iter) {
                if (fs::is_regular_file(*iter) && iter->extension() == ".png") {
                    cv::Mat P = projMats[camIdx++];
                    cv::Mat R, K2, t;
                    cv::decomposeProjectionMatrix(P, K2, R, t);

                    Camera cam(cv::imread(iter->string()));
                    cam.setCalibrationMatrix(K.empty() ? K2 : K);
                    if (!dist.empty()) {
                        cam.setDistortionCoeffs(dist);
                    }
                    cam.setProjectionMatrix(P);
                    cam.setRotationMatrix(R);
                    cam.setTranslationVector(t);

                    ds->addCamera(cam);
                }
            }
        }

        assert(ds->size() == numImages);
        return ds;
    }

    cv::Mat DataSetReader::loadMatrixFromFile(
        const std::string& filename, const std::string& matname) const {
        cv::Mat M;
        cv::FileStorage Fs(directory_ + filename, cv::FileStorage::READ);
        Fs[matname] >> M;
        return M;
    }

    std::vector<cv::Mat> DataSetReader::loadProjectionMatrices(
        const std::size_t numMatrices, const std::string& filename) const {

        std::vector<cv::Mat> projMats;
        boost::format fmt("viff%03d_matrix");
        cv::FileStorage fs(directory_ + filename, cv::FileStorage::READ);
        for (std::size_t idx = 0; idx < numMatrices; ++idx) {
            cv::Mat P;
            fs[(fmt % idx).str()] >> P;
            projMats.push_back(P);
        }

        return projMats;
    }

} // namespace io
} // namespace ret


//
//   Project Name:        Kratos
//   Last modified by:    $Author: pooyan $
//   Date:                $Date: 2008-07-09 13:12:21 $
//   Revision:            $Revision: 1.1 $
//
//

// System includes

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "spaces/ublas_space.h"

#include "linear_solvers/linear_solver.h"
#include "petsc_application.h"
#include "petsc_solver.h"

namespace Kratos
{

namespace Python
{
  void  AddPetscLinearSolversToPython()
  {
	typedef UblasSpace<double, CompressedMatrix, Vector> SpaceType;
	typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;

  	using namespace boost::python;

	typedef LinearSolver<SpaceType, LocalSpaceType> LinearSolverType;
	typedef PetscSolver<SpaceType, LocalSpaceType> PetscSolverType;
	class_<PetscSolverType, bases<LinearSolverType>, boost::noncopyable >
		( "PetscSolver",
			init<>() );
  }

}  // namespace Python.

} // Namespace Kratos


#include <iostream>

#include <armadillo>

#include <cpp-argparse/OptionParser.h>
#include <besiq/io/covariates.hpp>

#include <plink/plink_file.hpp>
#include <dcdflib/libdcdf.hpp>

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-var [OPTIONS] plink_file";
const std::string DESCRIPTION = "Computes variance hetrogenity for single variants.";
const std::string VERSION = "besiq 0.0.1";
const std::string EPILOG = "";

arma::mat
compute_medians(const snp_row &row, const arma::vec &pheno, const arma::uvec &missing)
{
    std::vector<std::vector<double>> groups( 3 );
    arma::mat medians = arma::zeros<arma::mat>( 3, 2 );
    for(int i = 0; i < row.size( ); i++)
    {
        if( row[ i ] != 3 && missing[ i ] == 0 )
        {
            groups[ row[ i ] ].push_back( pheno[ i ] );
            medians( row[ i ], 1 ) += 1.0;
        }
    }

    for(int i = 0; i < 3; i++)
    {
        std::sort( groups[ i ].begin( ), groups[ i ].end( ) );
        if( groups[ i ].size( ) > 0 )
        {
            medians( i, 0 ) = groups[ i ][ groups[ i ].size( ) / 2 ];
        }
        else
        {
            medians( i, 0 ) = 0.0;
        }
    }

    return medians;
}

bool
compute_brown_forsythe(const snp_row &row, const arma::vec &pheno, const arma::uvec &missing, double *W, double *p, size_t *N)
{
    double k = 3;
    arma::mat medians = compute_medians( row, pheno, missing );

    if( arma::min( medians.col( 1 ) ) <= 20 )
    {
        *N = arma::accu( medians.col( 1 ) );
        return false;
    }

    arma::vec z_i = arma::zeros<arma::vec>( 3 );
    double z = 0.0;
    *N = arma::accu( medians.col( 1 ) );
    for(int i = 0; i < row.size( ); i++)
    {
        if( row[ i ] != 3 && missing[ i ] == 0 )
        {
            double z_ij = std::abs( pheno[ i ] - medians( row[ i ], 0 ) );
            z_i[ row[ i ] ] += z_ij / medians( row[ i ], 1 );
            z += z_ij / *N;
        }
    }

    double W_sq = 0.0;
    for(int i = 0; i < row.size( ); i++)
    {
        if( row[ i ] != 3 && missing[ i ] == 0 )
        {
            double z_ij = std::abs( pheno[ i ] - medians( row[ i ], 0 ) );
            W_sq += pow( z_ij - z_i[ row[ i ] ], 2 );
        }
    }

    double numerator = dot( medians.col( 1 ), pow( z_i - z, 2 ) );

    *W = ( (*N - k) * numerator ) / ( ( k - 1 ) * W_sq );
    *p = 1 - f_cdf( *W, k - 1, *N - k );

    return true;
}

int
main(int argc, char *argv[])
{
    OptionParser parser = OptionParser( ).usage( USAGE )
                                         .version( VERSION )
                                         .description( DESCRIPTION )
                                         .epilog( EPILOG );

    parser.add_option( "-p", "--pheno" ).help( "Read phenotypes from this file instead of a plink file." );
    parser.add_option( "-e", "--mpheno" ).help( "Name of the phenotype that you want to read (if there are more than one in the phenotype file)." );
    parser.add_option( "-o", "--out" ).help( "The output file that will contain the results (binary)." );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 1 )
    {
        parser.print_help( );
        exit( 1 );
    }

    std::ios_base::sync_with_stdio( false );

    /* Read all genotypes */
    plink_file_ptr genotype_file = open_plink_file( parser.args( )[ 0 ] );
    genotype_matrix_ptr genotypes = create_genotype_matrix( genotype_file );
    std::vector<std::string> order = genotype_file->get_sample_iids( );

    /* Parse phenotypes */
    arma::uvec missing = arma::zeros<arma::uvec>( genotype_file->get_samples( ).size( ) );
    arma::vec phenotypes;
    if( options.is_set( "pheno" ) )
    {
        std::ifstream phenotype_file( options[ "pheno" ].c_str( ) );
        phenotypes = parse_phenotypes( phenotype_file, missing, order, options[ "mpheno" ] );
    }
    else
    {
        phenotypes = create_phenotype_vector( genotype_file->get_samples( ), missing );
    }

    std::vector<pio_locus_t> loci = genotype_file->get_loci( );
    std::cout << "chr\tpos\tsnp\tW\tP\tN\n";
    for( int i = 0; i < loci.size( ); i++)
    {
        const snp_row &row = *genotypes->get_row( loci[ i ].name );
        double W;
        double p;
        size_t N;
        if( compute_brown_forsythe( row, phenotypes, missing, &W, &p, &N ) )
        {
            std::cout << (int) loci[ i ].chromosome << "\t" << loci[ i ].bp_position << "\t"  << loci[ i ].name << "\t" << W << "\t" << p << "\t" << N << "\n";
        }
        else
        {
            std::cout << (int) loci[ i ].chromosome << "\t" << loci[ i ].bp_position << "\t" << loci[ i ].name << "\t" << "NA" << "\t" << "NA" << "\t" << N << "\n";
        }
    }

    return 0;
}


#include "tool/container/mapbox_vector_tile.hpp"

#include <protozero/pbf_writer.hpp>
#include <protozero/varint.hpp>

#include <boost/assert.hpp>

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#pragma GCC diagnostic ignored "-Wsign-conversion"
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#pragma GCC diagnostic pop

#include <algorithm>
#include <cmath>
#include <type_traits>

#include "geometric/coordinate.hpp"

namespace nepomuk
{
namespace tool
{
namespace container
{

namespace vector_tile
{
const constexpr std::uint32_t ID_TAG = 1;
const constexpr std::uint32_t NAME_TAG = 1;
const constexpr std::uint32_t FEATURE_TAG = 2;
const constexpr std::uint32_t LAYER_TAG = 3;
const constexpr std::uint32_t GEOMETRY_TAG = 3;
const constexpr std::uint32_t KEY_TAG = 3;
const constexpr std::uint32_t VARIANT_TAG = 4;
const constexpr std::uint32_t EXTENT_TAG = 5;
const constexpr std::uint32_t VERSION_TAG = 15;

const constexpr std::uint32_t FEATURE_ATTRIBUTES_TAG = 2;
const constexpr std::uint32_t FEATURE_GEOMETRIES_TAG = 4;

const constexpr std::uint32_t GEOMETRY_TYPE_POINT = 1;
const constexpr std::uint32_t GEOMETRY_TYPE_LINE = 2;

// https://github.com/mapbox/vector-tile-spec/blob/ab55f2bd7b8c0af5d9a845e4bb0b133811ce3ccf/2.1/vector_tile.proto#L17-L28
const constexpr std::uint32_t VARIANT_TYPE_STRING = 1;
const constexpr std::uint32_t VARIANT_TYPE_FLOAT = 2;
const constexpr std::uint32_t VARIANT_TYPE_DOUBLE = 3;

const constexpr std::uint32_t VARIANT_TYPE_UINT64 = 5;
const constexpr std::uint32_t VARIANT_TYPE_SINT64 = 6;
const constexpr std::uint32_t VARIANT_TYPE_BOOL = 7;

// Vector tiles are 4096 virtual pixels on each side
const constexpr double BUFFER = 128.0;
const constexpr double TILE_SIZE = 2 * BUFFER;
const constexpr double EXTENT = 16 * TILE_SIZE;

const constexpr int MOVETO_COMMAND = 9;
} // namespace vector_tile

namespace
{
template <typename content_type>
std::size_t get_map_index(std::map<content_type, std::size_t> &map, content_type const &content)
{
    auto const itr = map.find(content);
    if (itr == map.end())
    {
        auto result = map.size();
        map.insert(std::make_pair(content, result));
        return result;
    }
    else
        return itr->second;
}

using IPoint = boost::geometry::model::point<std::int64_t, 2, boost::geometry::cs::cartesian>;
using Linestring = boost::geometry::model::linestring<IPoint>;
using MultiLinestring = boost::geometry::model::multi_linestring<Linestring>;
using ClipBox = boost::geometry::model::box<IPoint>;

// clipping a single tile
const static ClipBox clip_box(IPoint(-vector_tile::BUFFER, -vector_tile::BUFFER),
                              IPoint(vector_tile::EXTENT + vector_tile::BUFFER,
                                     vector_tile::EXTENT + vector_tile::BUFFER));

IPoint to_tile_point(geometric::WGS84Coordinate coordiante,
                     geometric::MercatorBoundingBox const &bounding_box)
{
    geometric::MercatorCoordinate merc(std::move(coordiante));

    auto const px_longitude = (geometric::doubleFromLatLon(merc.longitude) -
                               geometric::doubleFromLatLon(bounding_box.lower_left.longitude)) /
                              bounding_box.width();
    auto const px_latitude = (geometric::doubleFromLatLon(merc.latitude) -
                              geometric::doubleFromLatLon(bounding_box.lower_left.latitude)) /
                             bounding_box.height();

    return {static_cast<std::int64_t>(std::llround(px_longitude * vector_tile::EXTENT)),
            static_cast<std::int64_t>(std::llround(px_latitude * vector_tile::EXTENT))};
}

} // namespace

bool VectorTileValue::operator==(VectorTileValue const &other) const
{
    // on equal types, we need to check the contained types, otherwise both entries are not empty
    if (type == other.type)
    {
        switch (type)
        {
        case VectorTileValueType::BOOL:
            return boost::get<bool>(value) == boost::get<bool>(other.value);
        case VectorTileValueType::INT:
            return boost::get<std::int64_t>(value) == boost::get<std::int64_t>(other.value);
        case VectorTileValueType::UINT:
            return boost::get<std::uint64_t>(value) == boost::get<std::uint64_t>(other.value);
        case VectorTileValueType::DOUBLE:
            return boost::get<double>(value) == boost::get<double>(other.value);
        case VectorTileValueType::FLOAT:
            return boost::get<float>(value) == boost::get<float>(other.value);
        default:
            BOOST_ASSERT(type == VectorTileValueType::STRING);
            return boost::get<std::string>(value) == boost::get<std::string>(other.value);
        }
    }
    else
        return false;
}

bool VectorTileValue::operator<(VectorTileValue const &other) const
{
    if (type == other.type)
    {
        switch (type)
        {
        case VectorTileValueType::BOOL:
            return boost::get<bool>(value) < boost::get<bool>(other.value);
        case VectorTileValueType::INT:
            return boost::get<std::int64_t>(value) < boost::get<std::int64_t>(other.value);
        case VectorTileValueType::UINT:
            return boost::get<std::uint64_t>(value) < boost::get<std::uint64_t>(other.value);
        case VectorTileValueType::DOUBLE:
            return boost::get<double>(value) < boost::get<double>(other.value);
        case VectorTileValueType::FLOAT:
            return boost::get<float>(value) < boost::get<float>(other.value);
        default:
            BOOST_ASSERT(type == VectorTileValueType::STRING);
            return boost::get<std::string>(value) < boost::get<std::string>(other.value);
        }
    }
    else
    {
        return type < other.type;
    }
}

void VectorTileValue::write(protozero::pbf_writer &pbf_writer) const
{
    protozero::pbf_writer values_writer(pbf_writer, vector_tile::VARIANT_TAG);

    switch (type)
    {
    case VectorTileValueType::BOOL:
        values_writer.add_bool(vector_tile::VARIANT_TYPE_BOOL, boost::get<bool>(value));
        break;
    case VectorTileValueType::INT:
        values_writer.add_int64(vector_tile::VARIANT_TYPE_SINT64, boost::get<std::int64_t>(value));
        break;
    case VectorTileValueType::UINT:
        values_writer.add_int64(vector_tile::VARIANT_TYPE_UINT64, boost::get<std::uint64_t>(value));
        break;
    case VectorTileValueType::DOUBLE:
        values_writer.add_double(vector_tile::VARIANT_TYPE_DOUBLE, boost::get<double>(value));
        break;
    case VectorTileValueType::FLOAT:
        values_writer.add_float(vector_tile::VARIANT_TYPE_FLOAT, boost::get<float>(value));
        break;
    default:
        BOOST_ASSERT(type == VectorTileValueType::STRING);
        values_writer.add_string(vector_tile::VARIANT_TYPE_STRING, boost::get<std::string>(value));
        break;
    }
}

MapboxVectorTileLayer::MapboxVectorTileLayer(protozero::pbf_writer &pbf_writer,
                                             std::string const &name,
                                             geometric::MercatorBoundingBox bounding_box)
    : feature_id(1), layer_writer(pbf_writer, vector_tile::LAYER_TAG),
      bounding_box(std::move(bounding_box))
{
    // each layer starts of with its version
    layer_writer.add_uint32(vector_tile::VERSION_TAG, 2);
    // Field 1 is the "layer name" field, it's a string
    layer_writer.add_string(vector_tile::NAME_TAG, name);
    // Field 5 is the tile extent. It's a uint32 and should be set to 4096
    layer_writer.add_uint32(vector_tile::EXTENT_TAG, vector_tile::EXTENT);
}

MapboxVectorTileLayer::~MapboxVectorTileLayer()
{
    // write the key/value tables
    write_key_table();
    write_value_table();
}

void MapboxVectorTileLayer::write_key_table()
{
    std::vector<std::string> ordered_keys(keys.size());
    for (auto const &pair : keys)
        ordered_keys[pair.second] = pair.first;

    auto const add_key_tag = [this](auto const &key) {
        layer_writer.add_string(vector_tile::KEY_TAG, key);
    };

    std::for_each(ordered_keys.begin(), ordered_keys.end(), add_key_tag);
}

void MapboxVectorTileLayer::write_value_table()
{
    std::vector<VectorTileValue> ordered_values(values.size());
    for (auto const &pair : values)
        ordered_values[pair.second] = pair.first;

    auto const call_write = [this](auto const &value) { value.write(layer_writer); };

    std::for_each(ordered_values.begin(), ordered_values.end(), call_write);
}

void MapboxVectorTileLayer::add_point(
    geometric::WGS84Coordinate const coordinate,
    std::vector<std::pair<std::string, VectorTileValue>> const &features)
{
    // only add the point, if it is within the tile
    auto const tile_point = to_tile_point(std::move(coordinate), bounding_box);
    if (!boost::geometry::within(tile_point, clip_box))
        return;

    protozero::pbf_writer feature_writer(layer_writer, vector_tile::FEATURE_TAG);
    feature_writer.add_enum(vector_tile::GEOMETRY_TAG, vector_tile::GEOMETRY_TYPE_POINT);
    feature_writer.add_uint64(vector_tile::ID_TAG, feature_id++);
    encode_features(feature_writer, features);

    // encode the point, scoping for flushing
    {
        protozero::packed_field_uint32 geometry(feature_writer,
                                                vector_tile::FEATURE_GEOMETRIES_TAG);
        geometry.add_element(vector_tile::MOVETO_COMMAND);
        geometry.add_element(protozero::encode_zigzag32(tile_point.get<0>()));
        geometry.add_element(protozero::encode_zigzag32(tile_point.get<1>()));
    }
}

void MapboxVectorTileLayer::add_line(
    std::vector<geometric::WGS84Coordinate> const &line_string,
    std::vector<std::pair<std::string, VectorTileValue>> const &features)
{
    if (line_string.size() <= 1)
        return;

    // clip the linestring to the tilebox
    Linestring unclipped_line;

    auto const append_tile_coordinate = [this,
                                         &unclipped_line](geometric::WGS84Coordinate coordinate) {
        auto const tile_point = to_tile_point(std::move(coordinate), bounding_box);
        boost::geometry::append(unclipped_line, tile_point);
    };
    std::for_each(line_string.begin(), line_string.end(), append_tile_coordinate);

    MultiLinestring clipped_line;
    boost::geometry::intersection(clip_box, unclipped_line, clipped_line);

    // only start encoding, if there is an actual line string within the box
    if (std::find_if(clipped_line.begin(), clipped_line.end(), [](auto const &line) {
            return line.size() >= 2;
        }) == clipped_line.end())
        return;

    protozero::pbf_writer feature_writer(layer_writer, vector_tile::FEATURE_TAG);
    feature_writer.add_enum(vector_tile::GEOMETRY_TAG, vector_tile::GEOMETRY_TYPE_LINE);
    feature_writer.add_uint64(vector_tile::ID_TAG, feature_id++);
    encode_features(feature_writer, features);

    for (auto const &line_segment : clipped_line)
    {
        // only encode lines
        if (line_segment.size() >= 2)
        {
            protozero::packed_field_uint32 geometry(feature_writer,
                                                    vector_tile::FEATURE_GEOMETRIES_TAG);
            geometry.add_element(vector_tile::MOVETO_COMMAND);
            std::int32_t cur_lon = 0, cur_lat = 0;
            // if this is auto, somehow it ends up as `expecting expression` to access the
            // coordinates
            auto const encode_point = [&cur_lon, &cur_lat, &geometry](IPoint const &tile_point) {
                geometry.add_element(protozero::encode_zigzag32(tile_point.get<0>() - cur_lon));
                geometry.add_element(protozero::encode_zigzag32(tile_point.get<1>() - cur_lat));
                cur_lon = tile_point.get<0>();
                cur_lat = tile_point.get<1>();
            };

            // encode the very first point
            encode_point(line_segment.front());

            // See:
            // https://github.com/mapbox/vector-tile-spec/tree/master/2.1#example-command-integers
            geometry.add_element(((line_segment.size() - 1) << 3) | 2u);
            std::for_each(line_segment.begin() + 1, line_segment.end(), encode_point);
        }
    }
}

void MapboxVectorTileLayer::encode_features(
    protozero::pbf_writer &feature_writer,
    std::vector<std::pair<std::string, VectorTileValue>> const &features)
{
    protozero::packed_field_uint32 field(feature_writer, vector_tile::FEATURE_ATTRIBUTES_TAG);

    auto const encode = [this, &field](auto const &entry) {
        auto const name_id = get_map_index(keys, entry.first);
        auto const value_id = get_map_index(values, entry.second);

        field.add_element(name_id);
        field.add_element(value_id);
    };

    std::for_each(features.begin(), features.end(), encode);
}

MapboxVectorTile::MapboxVectorTile(std::uint32_t const horizontal,
                                   std::uint32_t const vertical,
                                   std::uint32_t const zoom_level)
    : tile_writer(pbf_buffer), bounding_box(horizontal, vertical, zoom_level)
{
}

MapboxVectorTile::operator std::string() const { return pbf_buffer; }

MapboxVectorTileLayer MapboxVectorTile::new_layer(std::string const &name)
{
    return MapboxVectorTileLayer(tile_writer, name, bounding_box);
}

} // namespace container
} // namespace tool
} // namespace nepomuk


#include "onlinetree.h"
#include "onlinenodegini.h"
#include "onlinenodehyperplane.h"
#include <cmath>
#include "utilities.h"
#include <boost/foreach.hpp>

OnlineTree::OnlineTree(const HyperParameters &hp, int numFeatures) : m_hp( &hp )
{
    if (hp.useRandProj)
    {
        m_rootNode = OnlineNode::Ptr(new OnlineNodeHyperplane(hp,0,numFeatures));
    }
    else
    {
        m_rootNode = OnlineNode::Ptr(new OnlineNodeGini(hp,0,numFeatures));
    }
}

OnlineTree::~OnlineTree()
{
}

void OnlineTree::finalize(const matrix<float>& data,
                          matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                          std::vector<int>& forestOutOfBagVoteNum)
{
    // Fill the confidence of the forest
    for (int nSamp = 0; nSamp < m_hp->numLabeled; nSamp++)
    {
        if (m_hp->useSoftVoting)
        {
            for (int nClass = 0; nClass < m_hp->numClasses; nClass++)
            {
                forestConfidences(nSamp,nClass) += m_confidences(nSamp,nClass);
            }
        }
        else
        {
            forestConfidences(nSamp, m_predictions[nSamp])++;
        }
    }

    // Fill the out of bag confidences and vote count
    BOOST_FOREACH(int n, m_outOfBagSamples)
    {
        forestOutOfBagVoteNum[n]++;

        if (m_hp->useSoftVoting)
        {
            for (int nClass = 0; nClass < m_hp->numClasses; nClass++)
            {
                forestOutOfBagConfidences(n,nClass) += m_confidences(n,nClass);
            }
        }
        else
        {
            forestOutOfBagConfidences(n, m_predictions[n])++;
        }
    }

    // Clean up
    clean();
}

void OnlineTree::clean()
{
    m_confidences.clear();
}

void OnlineTree::train(const std::vector<float>& sample, const int label)
{
    // Initialize
    //initialize(m_hp->numLabeled);

    // Random Subsamples data according to bagratio
    //subSample(m_hp->numLabeled);

    // Train the root Node
    m_rootNode->train(sample,label);
    //evalOutOfBagSamples(data);

    if (m_hp->verbose)
    {
        cout << "Trained a tree with " << m_rootNode->numNodes() << " nodes." << endl;
        // cout << "Training error = " << computeError(labels) << ", in bag = ";
        // cout << computeError(labels, m_inBagSamples) << ", out of bag = ";
        // cout << computeError(labels, m_outOfBagSamples) <<  endl;
    }
}

void OnlineTree::evalOutOfBagSamples(const matrix<float>& data)
{
    m_rootNode->eval(data, m_outOfBagSamples, m_confidences, m_predictions);
}

double OnlineTree::eval(const std::vector<float>& sample, const int label)
{
    // Initialize
    return m_rootNode->eval(sample, label);

    //if (m_hp->verbose) {
    // cout << "Test error: " << computeError(labels) << endl;
    //}
}

void OnlineTree::initialize(const int numSamples)
{
    m_confidences.resize(numSamples, m_hp->numClasses);
    m_predictions.resize(numSamples);

    m_inBagSamples.clear();
    m_outOfBagSamples.clear();
}

void OnlineTree::subSample(const int numSamples)
{
    if (m_hp->useSubSamplingWithReplacement)
    {
        m_inBagSamples = subSampleWithReplacement(numSamples);
        m_outOfBagSamples = setDiff(m_inBagSamples, numSamples);
    }
    else
    {
        subSampleWithoutReplacement(numSamples, static_cast<int>(floor(numSamples * m_hp->bagRatio)),
                                    m_inBagSamples, m_outOfBagSamples);
    }
}

void OnlineTree::subSample(const int numSamples, const std::vector<double>& weights)
{
    std::vector<int> tmpInBag;
    if (m_hp->useSubSamplingWithReplacement)
    {
        tmpInBag = subSampleWithReplacement(numSamples);
    }
    else
    {
        subSampleWithoutReplacement(numSamples, static_cast<int>(floor(numSamples * m_hp->bagRatio)),
                                    tmpInBag, m_outOfBagSamples);
    }

    // Weight Trimming
    BOOST_FOREACH(int n, tmpInBag)
    {
        if (weights[n] > 1e-4)
        {
            m_inBagSamples.push_back(n);
        }
    }
    m_outOfBagSamples = setDiff(m_inBagSamples, numSamples);
}

double OnlineTree::computeError(const std::vector<int>& labels, const std::vector<int>& sampleIndeces)
{
    double error = 0.0;
    int sampleCount = 0;
    BOOST_FOREACH(int n, sampleIndeces)
    {
        if (sampleCount < (int) labels.size())
        {
            error += (m_predictions[n] != labels[n]) ? 1.0 : 0.0;
        }
        else
        {
            break;
        }

        sampleCount++;
    }
    return error/(double) labels.size();
}

double OnlineTree::computeError(const std::vector<int>& labels)
{
    double error = 0.0;
    std::vector<int>::const_iterator itr(m_predictions.begin());
    std::vector<int>::const_iterator labelItr(labels.begin()), labelEnd(labels.end());
    for (; labelItr != labelEnd; itr++, labelItr++)
    {
        error += (*itr != *labelItr) ? 1.0 : 0.0;
    }
    return error/(double) labels.size();
}

/*
std::vector<int> OnlineTree::getNodeLabels() {
  std::set<int> nodeLabels;
  m_rootNode->getLabels(nodeLabels);

  std::vector<int> out;
  std::set<int>::const_iterator itr = nodeLabels.begin(), end = nodeLabels.end();
  for (; itr != end; itr++) {
    out.push_back(*itr);
  }

  return out;
}
*/


#include "Icontroller.hpp"
#include <iostream>
#include <Eigen/Eigen>
#include "include/Sensor.h"
#include "include/SensorArray.h"
#include "math.h"
#include "lqr.cpp"
#include "m5op.h"
#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <unistd.h>
#include <ios>
#include <iostream>
#include <fstream>
#include <string>
using namespace std;
void mem_usage(double& vm_usage, double& resident_set) {
   vm_usage = 0.0;
   resident_set = 0.0;
   ifstream stat_stream("/proc/self/stat",ios_base::in); //get info from proc directory
   //create some variables to get info
   string pid, comm, state, ppid, pgrp, session, tty_nr;
   string tpgid, flags, minflt, cminflt, majflt, cmajflt;
   string utime, stime, cutime, cstime, priority, nice;
   string O, itrealvalue, starttime;
   unsigned long vsize;
   long rss;
   stat_stream >> pid >> comm >> state >> ppid >> pgrp >> session >> tty_nr
   >> tpgid >> flags >> minflt >> cminflt >> majflt >> cmajflt
   >> utime >> stime >> cutime >> cstime >> priority >> nice
   >> O >> itrealvalue >> starttime >> vsize >> rss; // don't care   about the rest
   stat_stream.close();
   long page_size_kb = sysconf(_SC_PAGE_SIZE) / 1024; // for x86-64 is configured to use 2MB pages
   vm_usage = vsize / 1024.0;
   resident_set = rss * page_size_kb;
}

int main()
	{
	double vm, rss;
   mem_usage(vm, rss);
   cout << "Virtual Memory: " << vm << "\nResident set size: " << rss << endl;
	simulator_msgs::SensorArray arraymsg;
	std::vector<double> out;
	std::vector<double> xref;
	std::vector<double> error;
	std::vector<double> x;
	int k=0;

//		m5_dump_stats(0,0);
//		m5_reset_stats(0,0);
//		Foo* foo1 = new Foo ();
//		std::cout << "test if main works" << std::endl;;
		teste* control = new teste();

//		m5_dump_stats(0,0);
//		m5_reset_stats(0,0);

		control->config();

//		m5_dump_stats(0,0);
//		simulator_msgs::Sensor msgstates;
//		arraymsg.header;
//		msgstates = arraymsg.values.at(0);

		while(k<20){

		//m5_reset_stats(0,0);
		out=control->execute(arraymsg);
		mem_usage(vm, rss);
  		cout << "Virtual Memory: " << vm << "\nResident set size: " << rss << endl;
		//m5_dump_stats(0,0);

		k++;
		}
	mem_usage(vm, rss);
   cout << "Virtual Memory: " << vm << "\nResident set size: " << rss << endl;
		return 0;
	}


#include <sstream>
#include <iomanip>
#include <boost/algorithm/string/replace.hpp>
#include "ItemTextKeys.hpp"
#include "StringTable.hpp"

using namespace std;

ItemTextKeys::ItemTextKeys()
{
}

ItemTextKeys::~ItemTextKeys()
{
}

string ItemTextKeys::get_value(const uint value)
{
  string msg = StringTable::get(ITEM_VALUE);
  boost::replace_first(msg, "%s", to_string(value));

  return msg;
}

string ItemTextKeys::get_nutrition_message(ConsumablePtr consumable)
{
  string msg = StringTable::get(NUTRITION_MESSAGE);
  int turns = 0;

  if (consumable != nullptr)
  {
    turns = consumable->get_nutrition();
  }

  boost::replace_first(msg, "%s", to_string(turns));
  return msg;
}

string ItemTextKeys::get_standard_drinks_message(ConsumablePtr consumable)
{
  string msg = StringTable::get(STANDARD_DRINKS_MESSAGE);
  string drink_cnt = "0";

  if (consumable != nullptr)
  {
    ostringstream ss;
    ss << std::setprecision(1) << consumable->get_standard_drinks();
    drink_cnt = ss.str();
  }

  boost::replace_first(msg, "%s", drink_cnt);
  return msg;
}

string ItemTextKeys::get_number_of_charges_message(WandPtr wand)
{
  string msg = StringTable::get(NUMBER_OF_CHARGES_MESSAGE);
  int charges = 0;

  if (wand != nullptr)
  {
    charges = wand->get_charges().get_current();
  }

  boost::replace_first(msg, "%s", to_string(charges));
  return msg;
}

string ItemTextKeys::get_wearable_protection_message(const int evade, const int soak)
{
  string msg = StringTable::get(WEARABLE_PROTECTION_MESSAGE);

  boost::replace_first(msg, "%s", to_string(evade));
  boost::replace_first(msg, "%s", to_string(soak));

  return msg;
}

string ItemTextKeys::get_speed_modifier_message(const int modifier)
{
  string msg;

  if (modifier >= 0)
  {
    msg = StringTable::get(SPEED_BONUS_MESSAGE);
  }
  else
  {
    msg = StringTable::get(SPEED_PENALTY_MESSAGE);
  }

  boost::replace_first(msg, "%s", to_string(std::abs(modifier)));

  return msg;
}

// Public
const string ItemTextKeys::ITEM_TRIED = "ITEM_TRIED";
const string ItemTextKeys::ITEM_GLOW = "ITEM_GLOW";
const string ItemTextKeys::ITEM_MULTIPLE = "ITEM_MULTIPLE";
const string ItemTextKeys::ITEM_CODEX_NOT_IDENTIFIED = "ITEM_CODEX_NOT_IDENTIFIED";
const string ItemTextKeys::ITEM_CODEX_NO_INFORMATION_FOUND = "ITEM_CODEX_NO_INFORMATION_FOUND";

// Protected
const string ItemTextKeys::ITEM_VALUE = "ITEM_VALUE";
const string ItemTextKeys::NUTRITION_MESSAGE = "NUTRITION_MESSAGE";
const string ItemTextKeys::STANDARD_DRINKS_MESSAGE = "STANDARD_DRINKS_MESSAGE";
const string ItemTextKeys::NUMBER_OF_CHARGES_MESSAGE = "NUMBER_OF_CHARGES_MESSAGE";
const string ItemTextKeys::WEARABLE_PROTECTION_MESSAGE = "WEARABLE_PROTECTION_MESSAGE";
const string ItemTextKeys::SPEED_BONUS_MESSAGE = "SPEED_BONUS_MESSAGE";
const string ItemTextKeys::SPEED_PENALTY_MESSAGE = "SPEED_PENALTY_MESSAGE";


#include "parse_opening_hours.hpp"
#include "opening_hours_parsers.hpp"

#include <boost/spirit/include/phoenix_bind.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>  // operator,

namespace osmoh
{
  namespace parsing
  {
    time_selector_parser::time_selector_parser() : time_selector_parser::base_type(main)
    {
      using qi::int_;
      using qi::_1;
      using qi::_2;
      using qi::_3;
      using qi::_a;
      using qi::_val;
      using qi::lit;
      using charset::char_;
      using osmoh::HourMinutes;
      using osmoh::TimeEvent;
      using osmoh::Time;
      using osmoh::Timespan;

      hour_minutes =
      (hours >> lit(':') >> minutes) [bind(&HourMinutes::AddDuration, _val, _1),
                                      bind(&HourMinutes::AddDuration, _val, _2)]
      ;

      extended_hour_minutes =
      (exthours >> lit(':') >> minutes)[bind(&HourMinutes::AddDuration, _val, _1),
                                        bind(&HourMinutes::AddDuration, _val, _2)]
      ;

      variable_time =
      ( lit('(')
       >> charset::no_case[event]         [bind(&TimeEvent::SetEvent, _val, _1)]
       >> ( (lit('+') >> hour_minutes)    [bind(&TimeEvent::SetOffset, _val, _1)]
           | (lit('-') >> hour_minutes)  [bind(&TimeEvent::SetOffset, _val, -_1)] )
       >> lit(')')
       )
      | charset::no_case[event][bind(&TimeEvent::SetEvent, _val, _1)]
      ;

      extended_time = extended_hour_minutes [bind(&Time::SetHourMinutes, _val, _1)]
      | variable_time                   [bind(&Time::SetEvent, _val, _1)]
      ;

      time = hour_minutes [bind(&Time::SetHourMinutes, _val, _1)]
      | variable_time [bind(&Time::SetEvent, _val, _1)]
      ;

      timespan =
      (time >> dash >> extended_time >> '/' >> hour_minutes)
      [bind(&Timespan::SetStart, _val, _1),
       bind(&Timespan::SetEnd, _val, _2),
       bind(&Timespan::SetPeriod, _val, _3)]

      | (time >> dash >> extended_time >> '/' >> minutes)
      [bind(&Timespan::SetStart, _val, _1),
       bind(&Timespan::SetEnd, _val, _2),
       bind(&Timespan::SetPeriod, _val, _3)]

      | (time >> dash >> extended_time >> '+')
      [bind(&Timespan::SetStart, _val, _1),
       bind(&Timespan::SetEnd, _val, _2),
       bind(&Timespan::SetPlus, _val, true)]

      | (time >> dash >> extended_time)
      [bind(&Timespan::SetStart, _val, _1),
       bind(&Timespan::SetEnd, _val, _2)]

      | (time >> '+')
      [bind(&Timespan::SetStart, _val, _1),
       bind(&Timespan::SetPlus, _val, true)]

      // This rule is only used for collection_times tag wish is not in our interest.
      // | time[bind(&Timespan::SetStart, _val, _1)]
      ;

      main %= timespan % ',';

      BOOST_SPIRIT_DEBUG_NODE(main);
      BOOST_SPIRIT_DEBUG_NODE(timespan);
      BOOST_SPIRIT_DEBUG_NODE(time);
      BOOST_SPIRIT_DEBUG_NODE(extended_time);
      BOOST_SPIRIT_DEBUG_NODE(variable_time);
      BOOST_SPIRIT_DEBUG_NODE(extended_hour_minutes);
    }
  }

  bool Parse(std::string const & str, TTimespans & context)
  {
    return osmoh::ParseImpl<parsing::time_selector_parser>(str, context);
  }
} // namespace osmoh


/*
 * Copyright 2014 RWTH Aachen University. All rights reserved.
 *
 * Licensed under the RWTH LM License (the "License");
 * you may not use this file except in compliance with the License.
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
#include <cmath>
#include <limits>
#include <sstream>
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/classification.hpp>
#define BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/filesystem/operations.hpp>
#undef BOOST_NO_CXX11_SCOPED_ENUMS
#include <boost/range/adaptor/reversed.hpp>
#include "identity.h"
#include "linear.h"
#include "lstm.h"
#include "net.h"
#include "output.h"
#include "sigmoid.h"
#include "softmax.h"
#include "tablelookup.h"
#include "tanh.h"

Net::Net(const ConstVocabularyPointer &vocabulary,
         const int max_batch_size,
         const int max_sequence_length,
         const int num_oovs,
         const bool is_feedforward,
         const Real learning_rate,
         const Real momentum,
         Random *random)
    : Function(1, 0, max_batch_size, max_sequence_length),
      num_oovs_(num_oovs),
      is_feedforward_(is_feedforward),
      vocabulary_(vocabulary),
      epoch_(0),
      learning_rate_(learning_rate),
      momentum_(momentum),
      best_perplexity_(std::numeric_limits<Real>::max()),
      random_(random) {
}

const Real *Net::Evaluate(const Slice &slice, const Real x[]) {
  for (FunctionPointer f : functions_)
    x = f->Evaluate(slice, x);
  return x;
}

void Net::ComputeDelta(const Slice &slice, FunctionPointer f) {
  for (FunctionPointer g : boost::adaptors::reverse(functions_)) {
    g->ComputeDelta(slice, f);
    f = g;
  }
}

const Real *Net::UpdateWeights(const Slice &slice, const Real x[]) {
  return UpdateWeights(slice, learning_rate(), x);
}

const Real *Net::UpdateWeights(const Slice &slice,
                               const Real learning_rate,
                               const Real x[]) {
  for (FunctionPointer f : functions_)
    x = f->UpdateWeights(slice, learning_rate, x);
  return x;
}

void Net::AddDelta(const Slice &slice, Real delta_t[]) {
  // not needed
}

void Net::Reset(const bool is_dependent) {
  assert(!is_dependent || max_batch_size() == 1);
  for (FunctionPointer f : functions_)
    f->Reset(is_dependent);
}

void Net::ExtractState(State *state) const {
  for (FunctionPointer f : functions_)
    f->ExtractState(state);
}

void Net::SetState(const State &state, const int i) {
  int j = 0;
  for (FunctionPointer f : functions_)
    f->SetState(state, j++);
}

void Net::RandomizeWeights(Random *random) {
  for (FunctionPointer f : functions_)
    f->RandomizeWeights(random);
}

void Net::Read(std::ifstream *input_stream) {
  input_stream->read(reinterpret_cast<char *>(&epoch_),
                     sizeof(int));
  input_stream->read(reinterpret_cast<char *>(&learning_rate_),
                     sizeof(Real));
  input_stream->read(reinterpret_cast<char *>(&best_perplexity_),
                     sizeof(Real));
  for (FunctionPointer f : functions_)
    f->Read(input_stream);
}

void Net::Write(std::ofstream *output_stream) {
  output_stream->write(reinterpret_cast<const char *>(&epoch_),
                       sizeof(int));
  output_stream->write(reinterpret_cast<const char *>(&learning_rate_),
                       sizeof(Real));
  output_stream->write(reinterpret_cast<const char *>(&best_perplexity_),
                       sizeof(Real));
  for (FunctionPointer f : functions_)
    f->Write(output_stream);
}

Real Net::ComputeLogProbability(const std::vector<int> &slice,
                                const Real x[],
                                const bool verbose,
                                ProbabilitySequenceVector *probabilities) {
  return functions_.back()->ComputeLogProbability(slice,
                                                  x,
                                                  verbose,
                                                  probabilities);
}

ActivationFunctionPointer Net::SetUpActivationFunction(const char type) const {
  ActivationFunctionPointer f;
  switch (type) {
  case '2':
  case '3':
  case '4':
  case '5':
  case '6':
  case '7':
  case '8':
  case '9':
  case 'i': f = ActivationFunctionPointer(new Identity());
    break;
  case 'l':
  case 'r': f = ActivationFunctionPointer(new Tanh());
    break;
  case 'L':
  case 'R': f = ActivationFunctionPointer(new Sigmoid());
    break;
  case 'm':
  case 'M':
    break;
  case 'x': f = ActivationFunctionPointer(new Softmax());
    break;
  default:
    assert(false);
  }
  return f;
}

FunctionPointer Net::SetUpFunction(const char type,
                                   const int dimension,
                                   const bool firstFunction,
                                   const bool use_bias,
                                   ActivationFunctionPointer g) const {
  FunctionPointer f;
  switch (type) {
  case '2':
  case '3':
  case '4':
  case '5':
  case '6':
  case '7':
  case '8':
  case '9':
  case 'i':
  case 'l':
  case 'L':
  case 'r':
  case 'R':
    if (firstFunction) {
        f = std::make_shared<TableLookup>(vocabulary_->GetVocabularySize(),
                                          dimension,
                                          max_batch_size(),
                                          max_sequence_length(),
                                          type >= '2' && type <= '9' ? type - '0' : 1,
                                          type == 'r' || type == 'R',
                                          use_bias,
                                          is_feedforward_,
                                          std::move(g));
    } else {
        f = std::make_shared<Linear>(output_dimension(),
                                     dimension,
                                     max_batch_size(),
                                     max_sequence_length(),
                                     type == 'r' || type == 'R',
                                     use_bias,
                                     std::move(g));
    }
    break;
  case 'm':
  case 'M':
    assert(!firstFunction);
    f = std::make_shared<LSTM>(output_dimension(),
                               dimension,
                               max_batch_size(),
                               max_sequence_length(),
                               use_bias);
    break;
  case 'x':
    f = std::make_shared<Output>(output_dimension(),
                                 max_batch_size(),
                                 max_sequence_length(),
                                 num_oovs_,
                                 use_bias,
                                 vocabulary_,
                                 std::move(g));
    break;
  default:
    assert(false);
  }
  return f;
}

void Net::BuildNetworkLayers(const std::string &net_config,
                             const bool use_bias) {
  std::vector<std::string> tokens;
  boost::split(tokens, net_config, boost::algorithm::is_any_of("-"));
  // first token is arbitrary name
  for (size_t i = 1; i < tokens.size(); ++i) {
    const char type = tokens[i][0];
    assert(type != 'x');
    std::stringstream converter(tokens[i].substr(1));
    int dimension;
    converter >> dimension;
    ActivationFunctionPointer g = SetUpActivationFunction(type);
    FunctionPointer f = SetUpFunction(type,
                                      dimension,
                                      i == 1,
                                      use_bias,
                                      std::move(g));
    Compose(f);
  }
  // add final softmax layer
  ActivationFunctionPointer g = SetUpActivationFunction('x');
  FunctionPointer f = SetUpFunction('x',
                                    vocabulary_->GetVocabularySize(),
                                    false,
                                    use_bias,
                                    std::move(g));
  Compose(f);
}

void Net::Read(const std::string &file_name) {
  assert(boost::filesystem::exists(file_name));
  std::ifstream file(file_name.c_str(), std::ios::in | std::ios::binary);
  assert(file.good());
  Read(&file);
  file.close();
}

void Net::Write(const std::string &file_name) {
  if (boost::filesystem::exists(file_name)) {
    boost::filesystem::copy_file(
        file_name, file_name + ".bk",
        boost::filesystem::copy_option::overwrite_if_exists);
  }
  std::ofstream file(file_name.c_str(), std::ios::out | std::ios::binary);
  Write(&file);
  file.close();
}