#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 <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;
}


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <orea/aggregation/dynamiccreditxvacalculator.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

using namespace std;
using namespace QuantLib;

using namespace boost::accumulators;

namespace ore {
namespace analytics {

DynamicCreditXvaCalculator::DynamicCreditXvaCalculator(
    //! Driving portfolio consistent with the cube below
    const boost::shared_ptr<Portfolio> portfolio, const boost::shared_ptr<Market> market,
    const string& configuration, const string& baseCurrency, const string& dvaName,
    const string& fvaBorrowingCurve, const string& fvaLendingCurve,
    const bool applyDynamicInitialMargin,
    const boost::shared_ptr<DynamicInitialMarginCalculator> dimCalculator,
    const boost::shared_ptr<NPVCube> tradeExposureCube,
    const boost::shared_ptr<NPVCube> nettingSetExposureCube,
    const boost::shared_ptr<NPVCube>& cptyCube,
    const Size tradeEpeIndex, const Size tradeEneIndex,
    const Size nettingSetEpeIndex, const Size nettingSetEneIndex, const Size cptySpIndex,
    const bool flipViewXVA, const string& flipViewBorrowingCurvePostfix, const string& flipViewLendingCurvePostfix)
    : ValueAdjustmentCalculator(portfolio, market, configuration, baseCurrency, dvaName,
                                fvaBorrowingCurve, fvaLendingCurve, applyDynamicInitialMargin,
                                dimCalculator, tradeExposureCube, nettingSetExposureCube, tradeEpeIndex, tradeEneIndex,
                                nettingSetEpeIndex, nettingSetEneIndex,
                                flipViewXVA, flipViewBorrowingCurvePostfix, flipViewLendingCurvePostfix),
      cptyCube_(cptyCube), cptySpIndex_(cptySpIndex) {
    // check consistency of input

    QL_REQUIRE(tradeExposureCube_->numDates() == cptyCube->numDates(),
        "number of dates in tradeExposureCube and cptyCube mismatch ("
        << tradeExposureCube_->numDates() << " vs " << cptyCube->numDates() << ")");

    QL_REQUIRE(cptySpIndex < cptyCube->depth(), "cptySpIndex("
        << cptySpIndex << ") exceeds depth of cptyCube("
        << cptyCube->depth() << ")");

    for (Size i = 0; i < tradeExposureCube_->numDates(); i++) {
        QL_REQUIRE(tradeExposureCube_->dates()[i] == cptyCube->dates()[i],
            "date at " << i << " in tradeExposureCube and cptyCube mismatch ("
            << tradeExposureCube_->dates()[i] << " vs " << cptyCube->dates()[i] << ")");
    }
}

const Real DynamicCreditXvaCalculator::calculateCvaIncrement(
    const string& tid, const string& cid, const Date& d0, const Date& d1, const Real& rr) {
    Real increment = 0.0;
    for (Size k = 0; k < tradeExposureCube_->samples(); ++k) {
        Real s0 = d0 == asof() ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = cptyCube_->get(cid, d1, k, cptySpIndex_);
        Real epe = tradeExposureCube_->get(tid, d1, k, tradeEpeIndex_);
        increment += (s0 - s1) * epe;
    }
    return (1.0 - rr) * increment / tradeExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateDvaIncrement(
    const string& tid, const Date& d0, const Date& d1, const Real& rr) {
    Real increment = 0.0;
    for (Size k = 0; k < tradeExposureCube_->samples(); ++k) {
        Real s0 = d0 == asof() ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real s1 = cptyCube_->get(dvaName_, d1, k, cptySpIndex_);
        Real ene = tradeExposureCube_->get(tid, d1, k, tradeEneIndex_);
        increment += (s0 - s1) * ene;
    }
    return (1.0 - rr) * increment / tradeExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateNettingSetCvaIncrement(
    const string& nid, const string& cid, const Date& d0, const Date& d1, const Real& rr) {
    Real increment = 0.0;
    for (Size k = 0; k < nettingSetExposureCube_->samples(); ++k) {
        Real s0 = d0 == asof() ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = cptyCube_->get(cid, d1, k, cptySpIndex_);
        Real epe = nettingSetExposureCube_->get(nid, d1, k, nettingSetEpeIndex_);
        increment += (s0 - s1) * epe;
    }
    return (1.0 - rr) * increment / nettingSetExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateNettingSetDvaIncrement(
    const string& nid, const Date& d0, const Date& d1, const Real& rr) {
    Real increment = 0.0;
    for (Size k = 0; k < nettingSetExposureCube_->samples(); ++k) {
        Real s0 = d0 == asof() ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real s1 = cptyCube_->get(dvaName_, d1, k, cptySpIndex_);
        Real ene = nettingSetExposureCube_->get(nid, d1, k, nettingSetEneIndex_);
        increment += (s0 - s1) * ene;
    }
    return (1.0 - rr) * increment / nettingSetExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateFbaIncrement(
    const string& tid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Real increment = 0.0;
    for (Size k = 0; k < tradeExposureCube_->samples(); ++k) {
        Real s0 = (d0 == asof() || cid == "") ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = (d0 == asof() || dvaName == "") ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real ene = tradeExposureCube_->get(tid, d1, k, tradeEneIndex_);
        increment += s0 * s1 * ene;
    }
    return increment * dcf / tradeExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateFcaIncrement(
    const string& tid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Real increment = 0.0;
    for (Size k = 0; k < tradeExposureCube_->samples(); ++k) {
        Real s0 = (d0 == asof() || cid == "") ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = (d0 == asof() || dvaName == "") ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real epe = tradeExposureCube_->get(tid, d1, k, tradeEpeIndex_);
        increment += s0 * s1 * epe;
    }
    return increment * dcf / tradeExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateNettingSetFbaIncrement(
    const string& nid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Real increment = 0.0;
    for (Size k = 0; k < nettingSetExposureCube_->samples(); ++k) {
        Real s0 = (d0 == asof() || cid == "") ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = (d0 == asof() || dvaName == "") ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real ene = nettingSetExposureCube_->get(nid, d1, k, nettingSetEneIndex_);
        increment += s0 * s1 * ene;
    }
    return increment * dcf / nettingSetExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateNettingSetFcaIncrement(
    const string& nid, const string& cid, const string& dvaName,
    const Date& d0, const Date& d1, const Real& dcf) {
    Real increment = 0.0;
    for (Size k = 0; k < nettingSetExposureCube_->samples(); ++k) {
        Real s0 = (d0 == asof() || cid == "") ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = (d0 == asof() || dvaName == "") ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real epe = nettingSetExposureCube_->get(nid, d1, k, nettingSetEpeIndex_);
        increment += s0 * s1 * epe;
    }
    return increment * dcf / nettingSetExposureCube_->samples();
}

const Real DynamicCreditXvaCalculator::calculateNettingSetMvaIncrement(
    const string& nid, const string& cid, const Date& d0, const Date& d1, const Real& dcf) {

    Real increment = 0.0;
    for (Size k = 0; k < nettingSetExposureCube_->samples(); ++k) {
        Real s0 = (d0 == asof() || cid == "") ? 1.0 : cptyCube_->get(cid, d0, k, cptySpIndex_);
        Real s1 = (d0 == asof() || dvaName_ == "") ? 1.0 : cptyCube_->get(dvaName_, d0, k, cptySpIndex_);
        Real im = dimCalculator_->dimCube()->get(nid, d1, k);
        increment += s0 * s1 * im;
    }
    return increment * dcf / nettingSetExposureCube_->samples();
}

} // namespace analytics
} // namespace ore


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef GM2CALC_INTERFACE_H
#define GM2CALC_INTERFACE_H

#include <Eigen/Core>

/**
 * @file gm2calc_interface.hpp
 * @brief contains declarations of GM2Calc interface functions
 */

namespace flexiblesusy {

/**
 * @class GM2Calc_data
 * @brief data to be passed to GM2Calc
 */
struct GM2Calc_data {
   GM2Calc_data();                ///< initializes members to GM2Calc default values
   void initialize();             ///< initializes members to GM2Calc default values

   double alpha_em_MZ;            ///< alpha_em(MZ)
   double alpha_em_0;             ///< alpha_em(0)
   double alpha_s_MZ;             ///< alpha_s(MZ) SM MS-bar
   double MZ;                     ///< Z pole mass
   double MW;                     ///< W pole mass
   double mb_mb;                  ///< mb(mb) SM MS-bar
   double MT;                     ///< top quark pole mass
   double MTau;                   ///< tau lepton pole mass
   double MM;                     ///< muon pole mass
   double MA0;                    ///< CP-odd Higgs pole mass
   double MSvm;                   ///< muon sneutrino pole mass
   Eigen::Array<double,2,1> MSm;  ///< smuon pole masses
   Eigen::Array<double,2,1> MCha; ///< chargino pole masses
   Eigen::Array<double,4,1> MChi; ///< neutralino pole masses
   double scale;                  ///< renormalization scale
   double TB;                     ///< tan(beta) DR-bar
   double Mu;                     ///< mu parameter (initial guess)
   double M1;                     ///< bino mass parameter (initial guess)
   double M2;                     ///< wino mass parameter (initial guess)
   double M3;                     ///< gluino mass parameter
   Eigen::Matrix<double,3,3> mq2; ///< left-handed squark mass parameters squared
   Eigen::Matrix<double,3,3> mu2; ///< right-handed up-type squark mass parameters squared
   Eigen::Matrix<double,3,3> md2; ///< right-handed down-type squark mass parameters squared
   Eigen::Matrix<double,3,3> ml2; ///< left-handed slepton mass parameters squared
   Eigen::Matrix<double,3,3> me2; ///< right-handed down-type slepton mass parameters squared
   Eigen::Matrix<double,3,3> Au;  ///< up-type squark trilinear coupling
   Eigen::Matrix<double,3,3> Ad;  ///< down-type squark trilinear coupling
   Eigen::Matrix<double,3,3> Ae;  ///< down-type slepton trilinear coupling
};

/// calculates amu using GM2Calc
double gm2calc_calculate_amu(const GM2Calc_data&);

/// calculates uncertainty of amu using GM2Calc
double gm2calc_calculate_amu_uncertainty(const GM2Calc_data&);

} // namespace flexiblesusy

#endif


#include "LanguageModel.h"

#include <iostream>
#include <math.h>
#include <random>
#include <sstream>

#include <boost/regex.hpp>

using namespace distrust;
using namespace Eigen;

LanguageModel::LanguageModel(const ModelInfo &model_info) :
  unif_(-1.0, 1.0),
  batch_size_(0) {

  window_size_ = model_info.window_size;
  wordvec_dim_ = model_info.wordvec_dim;
  hidden_dim_ = model_info.hidden_dim;
  start_token_index_ = model_info.start_token_index;
  end_token_index_ = model_info.end_token_index;
  unk_token_index_ = model_info.unk_token_index;
  vocab_size_ = model_info.vocab.size();
  for (unsigned int i = 0; i < vocab_size_; i++) {
    vocab_[model_info.vocab[i]] = i;
  }

  wordvec_w_.reserve(vocab_size_);
  wordvec_w_buf_.reserve(vocab_size_);
  input_hidden_w_.reserve(window_size_);
  input_hidden_w_buf_.reserve(window_size_);

  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_grad_.push_back(Matrix_t(hidden_dim_, wordvec_dim_));
  }
  input_hidden_b_grad_ = Vector_t(hidden_dim_);
  hidden_output_w_grad_ = Matrix_t(vocab_size_, hidden_dim_);
  hidden_output_b_grad_ = Vector_t(vocab_size_);

  // adagrad
  for (unsigned int i = 0; i < vocab_size_; i++) {
    wordvec_w_var_.push_back(ArrayXd::Zero(wordvec_dim_));
  }

  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_var_.push_back(MArray_t::Zero(hidden_dim_, wordvec_dim_));
  }

  input_hidden_b_var_ = ArrayXd::Zero(hidden_dim_);
  hidden_output_w_var_ = MArray_t::Zero(vocab_size_, hidden_dim_);
  hidden_output_b_var_ = ArrayXd::Zero(vocab_size_);
  zero_grad_params();
}

LanguageModel::LanguageModel(const LanguageModel &model) :
  window_size_(model.window_size_),
  wordvec_dim_(model.wordvec_dim_),
  hidden_dim_(model.hidden_dim_),
  start_token_index_(model.start_token_index_),
  end_token_index_(model.end_token_index_),
  unk_token_index_(model.unk_token_index_),
  vocab_size_(model.vocab_size_),
  vocab_(model.vocab_),
  wordvec_w_buf_(model.wordvec_w_buf_),
  input_hidden_w_buf_(model.input_hidden_w_buf_),
  input_hidden_b_buf_(model.input_hidden_b_buf_),
  hidden_output_w_buf_(model.hidden_output_w_buf_),
  hidden_output_b_buf_(model.hidden_output_b_buf_),
  input_hidden_w_grad_(model.input_hidden_w_grad_),
  input_hidden_b_grad_(model.input_hidden_b_grad_),
  hidden_output_w_grad_(model.hidden_output_w_grad_),
  hidden_output_b_grad_(model.hidden_output_b_grad_) {

  zero_grad_params();
  wrap_buffers();
}

double
LanguageModel::sample() {
  return unif_(re_);
}

void
LanguageModel::wrap_buffers() {
  wordvec_w_.clear();
  for (unsigned int i = 0; i < vocab_size_; i++) {
    wordvec_w_.push_back(Map<Vector_t>(&wordvec_w_buf_[i][0], wordvec_dim_));
  }

  input_hidden_w_.clear();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_.push_back(Map<Matrix_t>(&input_hidden_w_buf_[i][0], hidden_dim_, wordvec_dim_));
  }

  input_hidden_b_ = std::unique_ptr<Map<Vector_t>>(
    new Map<Vector_t>(&input_hidden_b_buf_[0], hidden_dim_));
  hidden_output_w_ = std::unique_ptr<Map<Matrix_t>>(
    new Map<Matrix_t>(&hidden_output_w_buf_[0], vocab_size_, hidden_dim_));
  hidden_output_b_ = std::unique_ptr<Map<Vector_t>>(
    new Map<Vector_t>(&hidden_output_b_buf_[0], vocab_size_));
}

void
LanguageModel::random_init() {
  wordvec_w_buf_.clear();
  for (unsigned int i = 0; i < vocab_size_; i++) {
    wordvec_w_buf_.push_back(std::vector<double>(wordvec_dim_));
    for (unsigned int j = 0; j < wordvec_dim_; j++) {
      wordvec_w_buf_[i][j] = 0.05 * sample();
    }
  }

  input_hidden_w_buf_.clear();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_buf_.push_back(std::vector<double>(hidden_dim_ * wordvec_dim_));
    for (unsigned int j = 0; j < hidden_dim_ * wordvec_dim_; j++) {
      input_hidden_w_buf_[i][j] = 0.05 * sample();
    }
  }

  input_hidden_b_buf_ = std::vector<double>(hidden_dim_, 0.0);
  hidden_output_w_buf_ = std::vector<double>(vocab_size_ * hidden_dim_, 0.0);
  hidden_output_b_buf_ = std::vector<double>(vocab_size_, 0.0);
  wrap_buffers();
}

void
LanguageModel::set_params(Params &params) {
  wordvec_w_buf_ = std::move(params.wordvec_w);
  input_hidden_w_buf_ = std::move(params.input_hidden_w);
  input_hidden_b_buf_ = std::move(params.input_hidden_b);
  hidden_output_w_buf_ = std::move(params.hidden_output_w);
  hidden_output_b_buf_ = std::move(params.hidden_output_b);
  wrap_buffers();
}

void
LanguageModel::update_params(const ParamUpdate &update) {
  for (auto itr = update.wordvec_w.begin(); itr != update.wordvec_w.end(); itr++) {
    uint32_t idx = itr->first;
    for (unsigned int i = 0; i < wordvec_dim_; i++) {
      wordvec_w_buf_[idx][i] += itr->second[i];
    }
  }

  for (unsigned int i = 0; i < window_size_; i++) {
    for (unsigned int j = 0; j < wordvec_dim_ * hidden_dim_; j++) {
      input_hidden_w_buf_[i][j] += update.input_hidden_w[i][j];
    }
  }

  for (unsigned int i = 0; i < hidden_dim_; i++) {
    input_hidden_b_buf_[i] += update.input_hidden_b[i];
  }

  for (unsigned int i = 0; i < hidden_dim_ * vocab_size_; i++) {
    hidden_output_w_buf_[i] += update.hidden_output_w[i];
  }

  for (unsigned int i = 0; i < vocab_size_; i++) {
    hidden_output_b_buf_[i] += update.hidden_output_b[i];
  }
}

void
LanguageModel::get_params(Params &ret) {
  ret.wordvec_w = wordvec_w_buf_;
  ret.input_hidden_w = input_hidden_w_buf_;
  ret.input_hidden_b = input_hidden_b_buf_;
  ret.hidden_output_w = hidden_output_w_buf_;
  ret.hidden_output_b = hidden_output_b_buf_;
}

void
LanguageModel::get_update(ParamUpdate &ret, const double learn_rate) {
  double *ptr;
  for (auto itr = wordvec_w_grad_.begin(); itr != wordvec_w_grad_.end(); itr++) {
    uint32_t idx = itr->first;
    //ArrayXd a = itr->second.array() * (-learn_rate);
    ArrayXd a = itr->second.array();
    wordvec_w_var_[idx] += a.square();
    a *= -learn_rate / (wordvec_w_var_[idx].sqrt() + 1e-4);
    ptr = a.data();
    ret.wordvec_w[idx] = std::vector<double>(ptr, ptr + wordvec_dim_);
  }

  for (unsigned int i = 0; i < window_size_; i++) {
    //MArray_t a = input_hidden_w_grad_[i].array() * (-learn_rate);
    MArray_t a = input_hidden_w_grad_[i].array();
    input_hidden_w_var_[i] += a.square();
    a *= -learn_rate / (input_hidden_w_var_[i].sqrt() + 1e-4);
    ptr = a.data();
    ret.input_hidden_w.push_back(std::vector<double>(ptr, ptr + hidden_dim_ * wordvec_dim_));
  }

  //ArrayXd a_ihb = input_hidden_b_grad_.array() * (-learn_rate);
  ArrayXd a_ihb = input_hidden_b_grad_.array();
  input_hidden_b_var_ += a_ihb.square();
  a_ihb *= -learn_rate / (input_hidden_b_var_.sqrt() + 1e-4);
  ptr = a_ihb.data();
  ret.input_hidden_b = std::vector<double>(ptr, ptr + hidden_dim_);

  //MArray_t a_how = hidden_output_w_grad_.array() * (-learn_rate);
  MArray_t a_how = hidden_output_w_grad_.array();
  hidden_output_w_var_ += a_how.square();
  a_how *= -learn_rate / (hidden_output_w_var_.sqrt() + 1e-4);
  ptr = a_how.data();
  ret.hidden_output_w = std::vector<double>(ptr, ptr + vocab_size_ * hidden_dim_);

  //ArrayXd a_hob = hidden_output_b_grad_.array() * (-learn_rate);
  ArrayXd a_hob = hidden_output_b_grad_.array();
  hidden_output_b_var_ += a_hob.square();
  a_hob *= -learn_rate / (hidden_output_b_var_.sqrt() + 1e-4);
  ptr = a_hob.data();
  ret.hidden_output_b = std::vector<double>(ptr, ptr + vocab_size_);
}

VectorXd
LanguageModel::tanh(const VectorXd &v) {
  ArrayXd a = (v.array() * 2).exp();
  return ((a - 1) / (a + 1)).matrix();
}

double
LanguageModel::logZ(const VectorXd &v) {
  return log(v.array().exp().sum());
}

uint32_t
LanguageModel::word_index(const std::string &word) {
  boost::regex re("[0-9]");
  std::string token = boost::regex_replace(word, re, "0");
  uint32_t index = unk_token_index_;
  auto itr = vocab_.find(token);
  if (itr != vocab_.end()) {
    index = itr->second;
  }
  return index;
}

std::vector<uint32_t>
LanguageModel::tokenize(const std::string &line) {
  std::vector<uint32_t> tokens;
  for (unsigned int i = 0; i < window_size_; i++) {
    tokens.push_back(start_token_index_);
  }

  std::string word;
  std::stringstream ss(line);
  while (std::getline(ss, word, ' ')) {
    tokens.push_back(word_index(word));
  }
  tokens.push_back(end_token_index_);
  return tokens;
}

std::vector<double>
LanguageModel::forward(const std::vector<uint32_t> &input) {
  if (input.size() != window_size_) {
    throw std::invalid_argument("forward: input size does not equal window size");
  }

  hidden_ = *input_hidden_b_;
  for (unsigned int i = 0; i < window_size_; i++) {
    hidden_ += input_hidden_w_[i] * wordvec_w_[input[i]];
  }
  hidden_tanh_ = tanh(hidden_);
  output_ = (*hidden_output_w_) * hidden_tanh_ + (*hidden_output_b_);
  logZ_ = logZ(output_);
  output_normed_ = (output_.array() - logZ_).matrix();
  double *ptr = output_normed_.data();
  return std::vector<double>(ptr, ptr + vocab_size_);
}

// double
// LanguageModel::forward(const std::vector<uint32_t> &tokens) {
//   uint32_t size = tokens.size() - window_size_;
//   Matrix<uint32_t, Dynamic, Dynamic> input(size, window_size_);
//   for (unsigned int i = 0; i < size; i++) {
//     for (unsigned int j = 0; j < window_size_; j++) {
//       input(i, j) = tokens[i + j];
//     }
//   }

// }

// double
// LanguageModel::forward(const std::vector<uint32_t> &input, const uint32_t target) {
//   if (input.size() != window_size_) {
//     throw std::invalid_argument("forward: input size does not equal window size");
//   }

//   hidden_ = *input_hidden_b_;
//   for (unsigned int i = 0; i < window_size_; i++) {
//     hidden_ += input_hidden_w_[i] * wordvec_w_[input[i]];
//   }
//   hidden_tanh_ = tanh(hidden_);
//   output_ = (*hidden_output_w_) * hidden_tanh_ + (*hidden_output_b_);
//   logZ_ = logZ(output_);
//   output_normed_ = (output_.array() - logZ_).matrix();
//   return output_normed_(target);
// }

// std::vector<double>
// LanguageModel::forward(const std::vector<uint32_t> &input, uint32_t target_idx) {
//   hidden_ = *input_hidden_b_;
//   for (unsigned int i = 0; i < window_size_; i++) {
//     hidden_ += input_hidden_w_[i] * wordvec_w_[input[target_idx - window_size_ + i]];
//   }
//   hidden_tanh_ = tanh(hidden_);
//   output_ = (*hidden_output_w_) * hidden_tanh_ + (*hidden_output_b_);
//   logZ_ = logZ(output_);
//   output_normed_ = (output_.array() - logZ_).matrix();

//   double *ptr = output_normed_.data();
//   return std::vector<double>(ptr, ptr + vocab_size_);
// }

void
LanguageModel::backward(
  const std::vector<uint32_t> &input,
  const uint32_t target) {

  batch_size_++;

  // hidden-output gradients
  Vector_t output_grad = output_normed_.array().exp().matrix();
  output_grad(target) -= 1;
  hidden_output_w_grad_ += output_grad * hidden_tanh_.transpose();
  hidden_output_b_grad_ += output_grad;

  // input-hidden gradients
  Vector_t hidden_grad = output_grad.transpose() * (*hidden_output_w_);
  hidden_grad = (hidden_grad.array() *
    (1 - hidden_tanh_.array().square()))
    .matrix();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_grad_[i] += hidden_grad * wordvec_w_[input[i]].transpose();
  }
  input_hidden_b_grad_ += hidden_grad;

  // word vector gradients
  for (unsigned int i = 0; i < window_size_; i++) {
    uint32_t idx = input[i];
    Vector_t input_grad = hidden_grad.transpose() * input_hidden_w_[i];
    auto itr = wordvec_w_grad_.find(idx);
    if (itr == wordvec_w_grad_.end()) {
      wordvec_w_grad_[idx] = input_grad;
    } else {
      wordvec_w_grad_[idx] += input_grad;
    }
  }
}

void
LanguageModel::zero_grad_params() {
  batch_size_ = 0;
  wordvec_w_grad_.clear();
  for (unsigned int i = 0; i < window_size_; i++) {
    input_hidden_w_grad_[i].setZero();
  }
  input_hidden_b_grad_.setZero();
  hidden_output_w_grad_.setZero();
  hidden_output_b_grad_.setZero();
}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    This program is free software: you can redistribute it and/or modify
|    it under the terms of the GNU General Public License as published by
|    the Free Software Foundation, either version 3 of the License, or
|    (at your option) any later version.
|
|    This program is distributed in the hope that it will be useful,
|    but WITHOUT ANY WARRANTY; without even the implied warranty of
|    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
|    GNU General Public License for more details.
|
|    You should have received a copy of the GNU General Public License
|    along with this program.  If not, see <http://www.gnu.org/licenses/>.
|
\**************************************************************************/

#include "main.hpp"
#include <hashclash/rng.hpp>
#include <hashclash/progress_display.hpp>
#include <hashclash/timer.hpp>
#include <boost/lexical_cast.hpp>

uint32 m[80];
uint32 m2[80];
uint32 Q[85];
uint32 Q2[85];
uint32 Qr30[85];
uint32 Q2r30[85];
uint32 dQ[85];
uint32 dT[85];
uint32 dR[85];
uint32 dF[80];

uint32 Qvaluemask[85];
uint32 Qvalue[85];
uint32 Qprev[85];
uint32 Qprev2[85];

void fill_tables(const sha1differentialpath& diffpath)
{
	for (int t = -4; t <= 80; ++t)
	{
		dF[t] = dQ[offset+t] = dT[offset+t] = dR[offset+t] = 0;
		Qvaluemask[offset+t] = Qvalue[offset+t] = 0;
		Qprev[offset+t] = Qprev2[offset+t] = 0;
	}

	// build tables
	for (int t = diffpath.tbegin(); t < diffpath.tend(); ++t)
	{
		dQ[offset+t] = diffpath[t].diff();
		Qprev[offset+t] = diffpath[t].prev() | diffpath[t].prevn();
		Qprev2[offset+t] = diffpath[t].prev2() | diffpath[t].prev2n();
		Qvaluemask[offset+t] = (~diffpath[t].set0()) | diffpath[t].set1()
							| diffpath[t].prev() | diffpath[t].prevn()
							| diffpath[t].prev2() | diffpath[t].prev2n();
		Qvalue[offset+t] = diffpath[t].set1() | diffpath[t].prevn() | diffpath[t].prev2n();
	}
	booleanfunction* F[4] = { &SHA1_F1_data, &SHA1_F2_data, &SHA1_F3_data, &SHA1_F4_data };
	for (int t = diffpath.tbegin()+4; t+1 < diffpath.tend(); ++t) {
		booleanfunction& FF = *F[t/20];
		uint32 dFt = 0;
		for (unsigned b = 0; b < 32; ++b)
			dFt += FF.outcome(diffpath[t-1][b],diffpath[t-2][(b+2)&31],diffpath[t-3][(b+2)&31])(0,b);
		dF[t] = dFt;
	}

	Q[0] = Qvalue[0]; Q2[0] = Q[0] + dQ[0];
	Q[1] = Qvalue[1]; Q2[1] = Q[1] + dQ[1];
	Q[2] = Qvalue[2]; Q2[2] = Q[2] + dQ[2];
	Q[3] = Qvalue[3]; Q2[3] = Q[3] + dQ[3];
	Q[4] = Qvalue[4]; Q2[4] = Q[4] + dQ[4];
}

unsigned analyze_tunnel(const sha1differentialpath& tunnel, bool verbose, bool deepanalyze)
{
#define AT_TEST_COUNT (1<<18) // 16
#define AT_MIN_PROB 0
#define AT_MIN_COUNT (AT_TEST_COUNT>>AT_MIN_PROB)

	vector< vector<unsigned> > qtbcnts(80, vector<unsigned>(32,0));
	static uint32 tcnts[80], qmask[80];
	static uint32 mmask[80], mset1[80];
	for (unsigned t = 0; t < 40; ++t) {
		tcnts[t] = 0;
		qmask[t] = maindiffpath[t].mask();
	}
	sha1differentialpath diffpath = tunnel;
	diffpath.get(-4);
	diffpath.get(80);
	fill_tables(diffpath);
	for (int t = 16; t < 80; ++t) {
		if (t-4 >= tunnel.tbegin() && t+1 < tunnel.tend()) {
			mmask[t] = ~tunnel.getme(t).mask;
			mset1[t] = tunnel.getme(t).set1conditions();
		} else {
			mmask[t] = ~uint32(0);
			mset1[t] = 0;
		}
	}

	for (unsigned k = 0; k < AT_TEST_COUNT; ++k) {
//		if (k == (AT_TEST_COUNT>>6) && double(tcnts[22])/double(k) >= 0.8) return 17;
		int t = -4;
		while (t < 17) {
			bool ok = false;
			for (unsigned j = 0; j < 3; ++j) {
				Q[offset+t] = (xrng128()&~Qvaluemask[offset+t]) ^ Qvalue[offset+t];
				if (t > -4)
					Q[offset+t] ^= Qprev[offset+t]&Q[offset+t-1];
				if (t > -3)
					Q[offset+t] ^= Qprev2[offset+t]&Q[offset+t-2];
				Q2[offset+t] = Q[offset+t] + dQ[offset+t];
				if (t >= 1) {
					m[t-1] = Q[offset+t] - (
						sha1_f1(Q[offset+t-2], rotate_left(Q[offset+t-3],30), rotate_left(Q[offset+t-4],30)) + sha1_ac[0] + rotate_left(Q[offset+t-1],5) + rotate_left(Q[offset+t-5],30)
						);
					m2[t-1] = Q2[offset+t] - (
						sha1_f1(Q2[offset+t-2], rotate_left(Q2[offset+t-3],30), rotate_left(Q2[offset+t-4],30)) + sha1_ac[0] + rotate_left(Q2[offset+t-1],5) + rotate_left(Q2[offset+t-5],30)
						);
					if ((m2[t-1]^m[t-1]) == diffpath.getme(t-1).mask) {
						ok = true;
						break;
					} else {
						if (verbose)
							cout << "(" << t << ":" << naf(m2[t-1]^m[t-1]) << "!=" << diffpath.getme(t-1) << ") " << flush;
						continue;
					}
				} else
					ok = true;
				break;
			}
			if (ok)
				++t;
			else
				t -= 4;
			if (t < -4) throw std::runtime_error("t < -4");
		}
		for (unsigned i = 16; i < 26; ++i) {
			m[i]=rotate_left(m[i-3] ^ m[i-8] ^ m[i-14] ^ m[i-16], 1);
			m2[i]=rotate_left(m2[i-3] ^ m2[i-8] ^ m2[i-14] ^ m2[i-16], 1);
			uint32 md = m[i]^m2[i];
			m[i] = (m[i]&mmask[i])^mset1[i];
			m2[i] = m[i] ^ md;
		}
		for (int t = 16; t < 20; ++t) {
			sha1_step_round1(t, Q, m);
			sha1_step_round1(t, Q2, m2);
		}
		for (int t = 20; t < 26; ++t) {
			sha1_step_round2(t, Q, m);
			sha1_step_round2(t, Q2, m2);
		}
		for (int t = 17; t < 27; ++t) {
			for (unsigned b = 0; b < 32; ++b)
				if ((Q2[offset+t]^Q[offset+t])&(1<<b))
					++qtbcnts[t][b];
		}
		int breakt = 80;
		for (int t = 17; t < 27; ++t) {
			if ((Q2[offset+t]^Q[offset+t])&(qmask[t]|0x00000000)) {
				breakt = t;
				for (int t1 = t; t1 < 27; ++t1)
					++tcnts[t1];
				break;
			}
		}
	}
	int tret = 80;
	for (int t = 17; t < 27; ++t)
	{
		if (tcnts[t] >= AT_MIN_COUNT) { tret = t; break; }
		if (double(tcnts[t]) >= double(AT_TEST_COUNT)*0.1) { tret = t; break; }
	}
	if (!deepanalyze || tret < mainparameters.mintunnelpov || tret == 80)
		return tret;
	cout << "Tunnel: " << endl;
	show_path(tunnel);
	cout << "  << ";
	for (int t1 = 17; t1 <= 25; ++t1)
		cout << t1 << ":" << ((double(100)*double(tcnts[t1]))/double(AT_TEST_COUNT)) << "% ";
	cout << ">>  " << flush;
	cout << tret << "b[";
	for (unsigned b = 0; b < 32; ++b)
		cout << b << ":" << unsigned((double(100)*double(qtbcnts[tret][b]))/double(AT_TEST_COUNT)) << "%" << (b==31?"]":",");
	cout << endl;
	return tret;
}

#define GOODBIT(i,t,b) ((goodqtb[i][t]>>b)&1)
#define BADBIT(i,t,b) ((badqtb[i][t]>>b)&1)
#define STAT_TEST(ba,go) ((double(ba)/double(ba+go))<0.6*(double(badqtb.size())/double(badqtb.size()+goodqtb.size())))
#define STAT_ANA(ba,go,s) if (STAT_TEST(ba,go)) { cout << "poss. bc: q_" << t << "[" << b << "]=" << s << ": \t" << (double(100*ba)/double(ba+go)) << "%" << endl; }
void stat_ana(vector< vector<uint32> >& goodqtb, vector< vector<uint32> >& badqtb
	, vector< vector<uint32> >& goodq2tb, vector< vector<uint32> >& badq2tb
	, int t, int b)
{
	unsigned g_zero = 0, g_one = 0, g_prev = 0, g_prevn = 0, g_prevr = 0, g_prevrn = 0, g_prev2 = 0, g_prev2n = 0;
	unsigned b_zero = 0, b_one = 0, b_prev = 0, b_prevn = 0, b_prevr = 0, b_prevrn = 0, b_prev2 = 0, b_prev2n = 0;
	for (unsigned i = 0; i < goodqtb.size(); ++i) {
		if (GOODBIT(i,t,b)==0) ++g_zero; else ++g_one;
		if (GOODBIT(i,t,b)==GOODBIT(i,t-1,b)) ++g_prev; else ++g_prevn;
		if (GOODBIT(i,t,b)==GOODBIT(i,t-1,((b+2)&31))) ++g_prevr; else ++g_prevrn;
		if (GOODBIT(i,t,b)==GOODBIT(i,t-2,((b+2)&31))) ++g_prev2; else ++g_prev2n;
	}
	for (unsigned i = 0; i < badqtb.size(); ++i) {
		if (BADBIT(i,t,b)==0) ++b_zero; else ++b_one;
		if (BADBIT(i,t,b)==BADBIT(i,t-1,b)) ++b_prev; else ++b_prevn;
		if (BADBIT(i,t,b)==BADBIT(i,t-1,((b+2)&31))) ++b_prevr; else ++b_prevrn;
		if (BADBIT(i,t,b)==BADBIT(i,t-2,((b+2)&31))) ++b_prev2; else ++b_prev2n;
	}
	double baseprob = double(badqtb.size())/double(badqtb.size()+goodqtb.size());
	STAT_ANA(b_zero,g_zero,"0");
	STAT_ANA(b_one,g_one,"1");
	STAT_ANA(b_prev,g_prev,"^");
	STAT_ANA(b_prevn,g_prevn,"!");
	STAT_ANA(b_prevr,g_prevr,"^r");
	STAT_ANA(b_prevrn,g_prevrn,"!r");
	STAT_ANA(b_prev2,g_prev2,"M");
	STAT_ANA(b_prev2n,g_prev2n,"#");
	if (b != 0) return;
	if (1) {
		map<sdr, pair<unsigned,unsigned> > dqtstat;
		for (unsigned i = 0; i < goodqtb.size(); ++i)
			++dqtstat[sdr(goodqtb[i][t],goodq2tb[i][t])].first;
		for (unsigned i = 0; i < badqtb.size(); ++i)
			++dqtstat[sdr(badqtb[i][t],badq2tb[i][t])].second;
		typedef map<sdr, pair<unsigned,unsigned> >::const_iterator dqtstatcit;
		vector<dqtstatcit> goodcits;
		unsigned goodcnt = 0, badcnt = 0;
		for (dqtstatcit cit = dqtstat.begin(); cit != dqtstat.end(); ++cit)
			if ((double(cit->second.second)/double(cit->second.first+cit->second.second))<0.6*baseprob) {
				goodcits.push_back(cit);
				goodcnt += cit->second.first;
				badcnt += cit->second.second;
			}
		if (goodcits.size() && double(goodcnt) > 0.25*double(goodqtb.size()) && goodcits.size()*2 < goodqtb.size()) {
			cout << "good dQ_" << t << " " << double(badcnt*100)/double(badcnt+goodcnt) << "% bad #=" << goodcits.size() << ":\t";
			for (unsigned i = 0; i < goodcits.size(); ++i)
				if (double(goodcits[i]->second.first) > 0.01*double(goodqtb.size()))
				cout << (goodcits[i]->first) << "(" << 100.0*double(goodcits[i]->second.first)/double(goodcnt) << "%) ";
			cout << endl;
		}
	}
	if (1) {
		map<sdr, pair<unsigned,unsigned> > dftstat;
		if (t < 20) {
			for (unsigned i = 0; i < goodqtb.size(); ++i) {
				uint32 F1 = sha1_f1(goodqtb[i][t-1], rotate_left(goodqtb[i][t-2],30), rotate_left(goodqtb[i][t-3],30));
				uint32 F2 = sha1_f1(goodq2tb[i][t-1], rotate_left(goodq2tb[i][t-2],30), rotate_left(goodq2tb[i][t-3],30));
				++dftstat[sdr(F1,F2)].first;
			}
			for (unsigned i = 0; i < badqtb.size(); ++i) {
				uint32 F1 = sha1_f1(badqtb[i][t-1], rotate_left(badqtb[i][t-2],30), rotate_left(badqtb[i][t-3],30));
				uint32 F2 = sha1_f1(badq2tb[i][t-1], rotate_left(badq2tb[i][t-2],30), rotate_left(badq2tb[i][t-3],30));
				++dftstat[sdr(F1,F2)].second;
			}
		} else {
			for (unsigned i = 0; i < goodqtb.size(); ++i) {
				uint32 F1 = sha1_f2(goodqtb[i][t-1], rotate_left(goodqtb[i][t-2],30), rotate_left(goodqtb[i][t-3],30));
				uint32 F2 = sha1_f2(goodq2tb[i][t-1], rotate_left(goodq2tb[i][t-2],30), rotate_left(goodq2tb[i][t-3],30));
				++dftstat[sdr(F1,F2)].first;
			}
			for (unsigned i = 0; i < badqtb.size(); ++i) {
				uint32 F1 = sha1_f2(badqtb[i][t-1], rotate_left(badqtb[i][t-2],30), rotate_left(badqtb[i][t-3],30));
				uint32 F2 = sha1_f2(badq2tb[i][t-1], rotate_left(badq2tb[i][t-2],30), rotate_left(badq2tb[i][t-3],30));
				++dftstat[sdr(F1,F2)].second;
			}
		}
		typedef map<sdr, pair<unsigned,unsigned> >::const_iterator dftstatcit;
		vector<dftstatcit> goodcits;
		unsigned goodcnt = 0, badcnt = 0;
		for (dftstatcit cit = dftstat.begin(); cit != dftstat.end(); ++cit)
			if ((double(cit->second.second)/double(cit->second.first+cit->second.second))<0.6*baseprob) {
				goodcits.push_back(cit);
				goodcnt += cit->second.first;
				badcnt += cit->second.second;
			}
		if (goodcits.size() && double(goodcnt) > 0.25*double(goodqtb.size()) && goodcits.size()*2 < goodqtb.size()) {
			cout << "good dF_" << t << " " << double(badcnt*100)/double(badcnt+goodcnt) << "% bad #=" << goodcits.size() << ":\t";
			for (unsigned i = 0; i < goodcits.size(); ++i)
				if (double(goodcits[i]->second.first) > 0.01*double(goodqtb.size()))
				cout << (goodcits[i]->first) << "(" << 100.0*double(goodcits[i]->second.first)/double(goodcnt) << "%) ";
			cout << endl;
		}
	}
}

unsigned analyze_bc_tunnel(const sha1differentialpath& tunnel)
{
#define ATLC_TEST_COUNT (1<<18) // 16
#define ATLC_MIN_PROB 0
#define ATLC_MIN_COUNT (ATLC_TEST_COUNT>>ATLC_MIN_PROB)
	unsigned tend = analyze_tunnel(tunnel, false, mainparameters.changebitstat);
	memset(m, 0, sizeof(uint32)*16);
	for (int i = 0; i < 16; ++i)
		if (i >= tunnel.tbegin() && i < tunnel.tend())
			m[i] = tunnel.getme(i).mask;
	for (unsigned i = 16; i < 32; ++i)
		m[i]=rotate_left(m[i-3] ^ m[i-8] ^ m[i-14] ^ m[i-16], 1);
//	if (tend < 21)
//		return tend;
	show_path(tunnel);
	cout << tend << endl;
	vector< vector<uint32> > dmes(80);
	unsigned tend2 = tend;
	uint64 mcnt = 1;
	for (unsigned i = 16; i < tend; ++i) {
		sdr tmp;
		tmp.mask = m[i];
		tmp.sign = 0;
		do {
			dmes[i].push_back(tmp.adddiff());
			tmp.sign += ~tmp.mask + 1;
			tmp.sign &= tmp.mask;
		} while (tmp.sign != 0);
		sort(dmes[i].begin(), dmes[i].end());
		dmes[i].erase( unique(dmes[i].begin(),dmes[i].end()), dmes[i].end());
		if ((mcnt * dmes[i].size()) > (1<<6)) {
			// don't let mcnt grow too big
			tend2 = i;
			break;
		}
		mcnt *= uint64(dmes[i].size());
	}
	cout << mcnt << endl;
	uint32 qmask[80];
	for (unsigned t = 0; t < 40; ++t)
		qmask[t] = maindiffpath[t].mask();

	for (unsigned mi = 0; mi < mcnt; ++mi) {
		vector< vector<uint32> > goodqtb, badqtb, goodq2tb, badq2tb;
		vector<unsigned> dmesindex(80,0);
		unsigned mi2 = mi;
		cout << mi << ": ";
		for (unsigned i = 16; i < tend2; ++i)
			if (dmes[i].size() > 1) {
				dmesindex[i] = mi2 % dmes[i].size();
				mi2 /= dmes[i].size();
				cout << i << naf(dmes[i][dmesindex[i]]);
			}
		cout << ": ";
		sha1differentialpath diffpath = tunnel;
		diffpath.get(-4);
		diffpath.get(80);
		fill_tables(diffpath);
		for (unsigned k = 0; k < ATLC_TEST_COUNT; ++k) {
			int t = -4;
			while (t < 17) {
				bool ok = false;
				for (unsigned j = 0; j < 3; ++j) {
					Q[offset+t] = (xrng128()&~Qvaluemask[offset+t]) ^ Qvalue[offset+t];
					if (t > -4)
						Q[offset+t] ^= Qprev[offset+t]&Q[offset+t-1];
					if (t > -3)
						Q[offset+t] ^= Qprev2[offset+t]&Q[offset+t-2];
					Q2[offset+t] = Q[offset+t] + dQ[offset+t];
					if (t >= 1) {
						m[t-1] = Q[offset+t] - (
							sha1_f1(Q[offset+t-2], rotate_left(Q[offset+t-3],30), rotate_left(Q[offset+t-4],30)) + sha1_ac[0] + rotate_left(Q[offset+t-1],5) + rotate_left(Q[offset+t-5],30)
							);
						m2[t-1] = Q2[offset+t] - (
							sha1_f1(Q2[offset+t-2], rotate_left(Q2[offset+t-3],30), rotate_left(Q2[offset+t-4],30)) + sha1_ac[0] + rotate_left(Q2[offset+t-1],5) + rotate_left(Q2[offset+t-5],30)
							);
						if ((m2[t-1]^m[t-1]) == diffpath.getme(t-1).mask) {
							ok = true;
							break;
						} else {
							continue;
						}
					} else
						ok = true;
					break;
				}
				if (ok)
					++t;
				else
					t -= 4;
				if (t < -4) throw std::runtime_error("t < -4");
			}
			for (unsigned i = 16; i < 26; ++i) {
				m[i]=rotate_left(m[i-3] ^ m[i-8] ^ m[i-14] ^ m[i-16], 1);
				if (i < tend2)
					m2[i] = m[i] + dmes[i][dmesindex[i]];
				else
					m2[i]=rotate_left(m2[i-3] ^ m2[i-8] ^ m2[i-14] ^ m2[i-16], 1);
			}
			for (int t = 16; t < 20; ++t) {
				sha1_step_round1(t, Q, m);
				sha1_step_round1(t, Q2, m2);
			}
			for (int t = 20; t < 26; ++t) {
				sha1_step_round2(t, Q, m);
				sha1_step_round2(t, Q2, m2);
			}
			for (int t = 17; t < 27; ++t) {
				if ((Q2[offset+t]^Q[offset+t])&(qmask[t]|0x00000000)) {
					static vector<uint32> tmpqtb(27);
					for (int t1 = 0; t1 < tmpqtb.size(); ++t1)
						tmpqtb[t1] = Q[4+t1];
					if (t <= tend)
						badqtb.push_back(tmpqtb);
					else
						goodqtb.push_back(tmpqtb);
					for (int t1 = 0; t1 < tmpqtb.size(); ++t1)
						tmpqtb[t1] = Q2[4+t1];
					if (t <= tend)
						badq2tb.push_back(tmpqtb);
					else
						goodq2tb.push_back(tmpqtb);
					break;
				}
			}
		}
		cout << goodqtb.size() << " vs " << badqtb.size() << ": " << (double(100)*double(badqtb.size()))/double(goodqtb.size()+badqtb.size()) << "% bad" << endl;
		for (unsigned t = 16-4; t < tend; ++t)
			for (unsigned b = 0; b < 32; ++b) {
				stat_ana(goodqtb, badqtb, goodq2tb, badq2tb, t, b);
			}
	}
	return tend;
}

void find_best_tunnel(vector< sha1differentialpath >& tunnels, const sha1differentialpath& mypath, const vector< vector<uint32> >& bitrels, bool force_lc = false)
{
//#define ADD_ME_CARRIES 0
	unsigned ADD_ME_CARRIES = mainparameters.mecarry;
	if (mainparameters.tunnelfilterbitcondition)
		filter_tunnels_bitconditions(tunnels, mypath);
	static vector<sdr> sdrsvec[16];
	sdr metbu;
	static vector<unsigned> iQvec, ncvec;
	iQvec.assign(tunnels.size(),0);
	ncvec.assign(tunnels.size(),1<<20);
//	cout << tunnels.size() << endl;
	for (unsigned i = 0; i < tunnels.size(); ++i) {
		uint64 cnt = 1;
		for (int t = 0; t < 16; ++t) {
			sdrsvec[t].clear();
			if (t < tunnels[i].tbegin() || t >= tunnels[i].tend()) continue;
			metbu = tunnels[i].getme(t);
			table_sdrs(sdrsvec[t], metbu.adddiff(), hw(metbu.mask)+ADD_ME_CARRIES);
			unsigned j = 0;
			while (j < sdrsvec[t].size()) {
				if (sdrsvec[t][j].get(31) == -1) {
					swap(sdrsvec[t][j], sdrsvec[t][sdrsvec[t].size()-1]);
					sdrsvec[t].pop_back();
				} else
					++j;
			}
			cnt *= sdrsvec[t].size();
		}
		if (cnt == 0)
			throw std::runtime_error("cnt == 0 ??");
		unsigned bestiQ = 0, bestnc = 1<<20;
		uint64 bestc = cnt;
		unsigned basenc = tunnels[i].nrcond();
		for (uint64 c = 0; c < cnt; ++c) {
			uint64 cc = c;
			for (int t = 0; t < 16; ++t) {
				if (sdrsvec[t].size()>1) {
					uint64 k = cc % sdrsvec[t].size();
					cc /= sdrsvec[t].size();
					tunnels[i].getme(t) = sdrsvec[t][k];
				}
			}
			if (!filter_tunnel_bitrelations(tunnels[i],bitrels)) continue;
			unsigned iQ;
			if (force_lc)
				iQ = analyze_bc_tunnel(tunnels[i]);
			else
				iQ = analyze_tunnel(tunnels[i], false, mainparameters.changebitstat);
			if (iQ < bestiQ) continue;
			if (iQ > bestiQ) {
				bestiQ = iQ;
				bestnc = 1<<20;
				bestc = c;
			}
			unsigned nc = basenc;
			for (int t = tunnels[i].tbegin(); t < tunnels[i].tend(); ++t)
				nc += hw(tunnels[i].getme(t).mask&0x7FFFFFFF);
//			if (nc >= 15) continue;
			if (nc < bestnc) {
				bestnc = nc;
				bestc = c;
			}
		}
		if (bestc == cnt) {
			tunnels[i].clear();
			continue;
		}
		uint64 cc = bestc;
		for (int t = 0; t < 16; ++t) {
			if (sdrsvec[t].size()) {
				uint64 k = cc % sdrsvec[t].size();
				cc /= sdrsvec[t].size();
				tunnels[i].getme(t) = sdrsvec[t][k];
			}
		}
		iQvec[i] = bestiQ;
		ncvec[i] = bestnc;
//		if (bestiQ >= 23) break;
	}
	unsigned bestiQ = 0, bestnc = 1<<20, besti = tunnels.size(), minnc = 1<<20;
	for (unsigned i = 0; i < tunnels.size(); ++i) {
		if (tunnels[i].path.size()) {
			if (iQvec[i] > bestiQ) {
				bestiQ = iQvec[i];
				bestnc = ncvec[i];
				besti = i;
			} else if (iQvec[i] == bestiQ && ncvec[i] < bestnc) {
				bestnc = ncvec[i];
				besti = i;
			}
			if (ncvec[i] < minnc)
				minnc = ncvec[i];
		}
	}
	if (besti < tunnels.size()) {
//		if (bestiQ >= 21)
			cout << endl << bestiQ << "(" << bestnc << "," << minnc << ") " << flush;
		if (bestiQ >= 16) { //21
			//show_path(tunnels[besti]);
#if 0
			analyze_tunnel(tunnels[besti], false, true);
#else
			for (unsigned j = 0; j < tunnels.size(); ++j)
				if (iQvec[j] == iQvec[besti])
					analyze_tunnel(tunnels[j], false, true);
#endif
		}
		sha1differentialpath bestpath;
		bestpath.swap(tunnels[besti]);
		tunnels.resize(1);
		bestpath.swap(tunnels[0]);

		if (bestiQ < 19) tunnels.clear();

	} else
		tunnels.clear();
}

void create_tunnel_stept(vector< sha1differentialpath >& tunnels, sha1differentialpath& tunnel, int t, bool force_lc = false)
{
	if (t >= 0 && t+1 >= tunnel.tend()) {
		create_tunnel_stept(tunnels, tunnel, t-1, force_lc);
		return;
	}

	if (t-4 < tunnel.tbegin() || t < 0) {
		tunnels.push_back(tunnel);
		return;
	}
	if (tunnel[t-1].diff() == 0 && tunnel[t-2].diff() == 0 && tunnel[t-3].diff() == 0) {
		// dFt = 0
		uint32 dme = tunnel[t+1].diff() - tunnel[t].getsdr().rotate_left(5).adddiff() - tunnel[t-4].getsdr().rotate_left(30).adddiff();
		tunnel.getme(t) = naf(dme);
		create_tunnel_stept(tunnels, tunnel, t-1, force_lc);
		return;
	}

	bf_conditions newconditions[32][2];
	bool bitactive[32];
	unsigned cnt = 1;
	for (unsigned b = 0; b < 32; ++b) {
		bitactive[b] = false;
		//sha1_f1(Q[offset+t-1], rotate_left(Q[offset+t-2],30), rotate_left(Q[offset+t-3],30))
		bf_outcome bo = SHA1_F1_data.outcome(tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31));
		if (bo.size() == 0) throw std::runtime_error("bo.size() == 0");
		if (bo.size() == 1) continue;
		if (bo.size() == 2) {
			if (/*b == 31 &&*/ bo[0] != bc_constant && bo[1] != bc_constant)
				continue;
			if (force_lc) {
				bf_conditions bf;
				if (bo[0] != bc_constant)
					bf = SHA1_F1_data.backwardconditions( tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31), bo[0] );
				else
					bf = SHA1_F1_data.backwardconditions( tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31), bo[1] );
				tunnel.setbitcondition(t-1,b,bf.first);
				tunnel.setbitcondition(t-2,(b+2)&31,bf.second);
				tunnel.setbitcondition(t-3,(b+2)&31,bf.third);
				continue;
			}
			newconditions[b][0] = SHA1_F1_data.backwardconditions( tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31), bo[0]);
			newconditions[b][1] = SHA1_F1_data.backwardconditions( tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31), bo[1]);
			cnt *= 2;
			bitactive[b] = true;
		} else { // bo.size() == 3
			bf_conditions bf = SHA1_F1_data.backwardconditions( tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31), bc_constant );
#if 1
			if (mainparameters.simpletunnels) {
				tunnel.setbitcondition(t-1,b,bf.first);
				tunnel.setbitcondition(t-2,(b+2)&31,bf.second);
				tunnel.setbitcondition(t-3,(b+2)&31,bf.third);
				continue;
			}
#endif
			if (force_lc) {
				tunnel.setbitcondition(t-1,b,bf.first);
				tunnel.setbitcondition(t-2,(b+2)&31,bc_prevn);
				tunnel.setbitcondition(t-3,(b+2)&31,bf.third);
				continue;
			}
			if (bf.second != bc_prev) throw std::runtime_error("bf.second != bc_prev");
			newconditions[b][0] = bf;
			bf.second = bc_prevn;
			newconditions[b][1] = bf;
			cnt *= 2;
			bitactive[b] = true;
		}
	}
	for (unsigned c = 0; c < cnt; ++c) {
		unsigned cc = c;
		uint32 dFt = 0;
		for (unsigned b = 0; b < 32; ++b) {
			if (bitactive[b]) {
				unsigned k = cc % 2;
				cc /= 2;
				tunnel.setbitcondition(t-1,b,newconditions[b][k].first);
				tunnel.setbitcondition(t-2,((b+2)&31),newconditions[b][k].second);
				tunnel.setbitcondition(t-3,((b+2)&31),newconditions[b][k].third);
			}
			dFt += SHA1_F1_data.outcome(tunnel(t-1,b), tunnel(t-2,(b+2)&31), tunnel(t-3,(b+2)&31))(0,b);
		}

		uint32 dme = tunnel[t+1].diff() - dFt - tunnel[t].getsdr().rotate_left(5).adddiff() - tunnel[t-4].getsdr().rotate_left(30).adddiff();
		tunnel.getme(t) = naf(dme);
		create_tunnel_stept(tunnels, tunnel, t-1, force_lc);
	}
}

void analyze_tunnels_diffpath(const sha1differentialpath& mypath, const vector< vector<uint32> >& bitrels, vector<sha1differentialpath> tunnelsin)
{
	cout << "Analyzing tunnels for differential path:" << endl;
	sha1differentialpath path = mypath;
	cleanup_path(path);
	show_path(path);
	maindiffpath = path;
	cout << "Message space determined by " << bitrels.size() << " bitrelations." << endl;

	bool deep_analysis = mainparameters.tunneldeepanalysis;

	if (tunnelsin.size()) {
		vector<sha1differentialpath> tunnelstmp;
		for (unsigned i = 0; i < tunnelsin.size(); ++i)
			create_tunnel_stept(tunnelstmp, tunnelsin[i], 15);
		find_best_tunnel(tunnelstmp, path, bitrels, deep_analysis);
		return;
	}

	sha1differentialpath tunnelpath;
	vector< sha1differentialpath > tunnels, tunnelstmp, tunnelstmp2;

	cout << "Generating tunnels..." << endl;
	set<int> goodbits;
	set< pair<int,int> > good2bits;
#if 1
	for (int bit1 = 0; bit1 < 16*32; ++bit1) {
		int t1 = bit1 / 32;
		int b1 = bit1 % 32;
		tunnelpath.clear();
		tunnelpath.get(t1-4);
		if (t1 + 6 < 16)
			tunnelpath.get(t1+6);
		else
			tunnelpath.get(16);
		tunnelpath.setbitcondition(t1+1,b1,bc_plus);
		tunnelstmp.clear();
		create_tunnel_stept(tunnelstmp, tunnelpath, 15);
		find_best_tunnel(tunnelstmp, path, bitrels, deep_analysis);
		if (tunnelstmp.size()) {
			goodbits.insert(bit1);
			continue;
		}
	}
#if 1
	for (int bit1 = 0; bit1 < 16*32; ++bit1) {
		for (int bit2 = bit1+1; bit2 < 16*32; ++bit2) {
			if (goodbits.find(bit1) != goodbits.end()) continue;
			if (goodbits.find(bit2) != goodbits.end()) continue;
			int t1 = bit1 / 32;
			int b1 = bit1 % 32;
			int t2 = bit2/32;
			int b2 = bit2%32;
			tunnelpath.clear();
			tunnelpath.get(t1-4);
			if (t2 + 6 < 16)
				tunnelpath.get(t2+6);
			else
				tunnelpath.get(16);
			tunnelpath.setbitcondition(t1+1,b1,bc_plus);
			tunnelpath.setbitcondition(t2+1,b2,bc_plus);
			tunnelstmp.clear();
			create_tunnel_stept(tunnelstmp, tunnelpath, 15);
			find_best_tunnel(tunnelstmp, path, bitrels, deep_analysis);
			if (tunnelstmp.size())
				good2bits.insert(pair<int,int>(bit1,bit2));
		}
	}
#endif
//	exit(0);
#endif
	for (int bit1 = 16*32-1; bit1 >= 0; --bit1) {
//	for (int bit1 = 169; bit1 < 16*32; ++bit1) {
		cout << "[" << bit1 << "] " << flush;
		for (int bit2 = bit1; bit2 < 16*32; ++bit2) {
			for (int bit2sign = 0; bit2sign < 1; ++bit2sign)
			for (int bit3 = bit2; bit3 < 16*32; ++bit3)
			for (int bit3sign = 0; bit3sign < 1; ++bit3sign)
			{
				//if (bit2 != bit1 || bit3 != bit1) continue; // force single bit difference
				if (!mainparameters.threebittunnels && bit3 != bit2) continue; // force max 2 bit difference
				if (bit1 == bit2 && bit2sign == 1) continue;
				if (bit3 == bit2 && bit3sign != bit2sign) continue;
				if (bit1 == bit2 && bit3 > bit2) continue;
				if (goodbits.find(bit1) != goodbits.end()) continue;
				if (goodbits.find(bit2) != goodbits.end()) continue;
				if (goodbits.find(bit3) != goodbits.end()) continue;
				if (good2bits.find(pair<int,int>(bit1,bit2)) != good2bits.end()) continue;
				if (good2bits.find(pair<int,int>(bit1,bit3)) != good2bits.end()) continue;
				if (good2bits.find(pair<int,int>(bit2,bit3)) != good2bits.end()) continue;

				//cout << "[" << bit1 << "," << bit2 << "] " << flush;
				int t1 = bit1 / 32;
				int b1 = bit1 % 32;
				int t2 = bit2 / 32;
				int b2 = bit2 % 32;
				int t3 = bit3 / 32;
				int b3 = bit3 % 32;
				tunnelpath.clear();
				tunnelpath.get(t1-4);
				if (t3 + 6 < 16)
					tunnelpath.get(t3+6);
				else
					tunnelpath.get(16);
				tunnelpath.setbitcondition(t1+1,b1,bc_plus);
				if (bit2sign == 0)
					tunnelpath.setbitcondition(t2+1,b2,bc_plus);
				else
					tunnelpath.setbitcondition(t2+1,b2,bc_minus);
				if (bit3sign == 0)
					tunnelpath.setbitcondition(t3+1,b3,bc_plus);
				else
					tunnelpath.setbitcondition(t3+1,b3,bc_minus);
				tunnelstmp.clear();
				create_tunnel_stept(tunnelstmp, tunnelpath, 15);
				find_best_tunnel(tunnelstmp, path, bitrels, deep_analysis);
			}
		}
	}
	exit(0);
}

struct ihv_type {
	uint32 ihv[5];
	bool operator<(const ihv_type& r) const {
		for (int i = 0; i < 4; ++i) {
			if (ihv[i] < r.ihv[i]) return true;
			if (ihv[i] > r.ihv[i]) return false;
		}
		if (ihv[4] < r.ihv[4]) return true;
		return false;
	}
	bool operator==(const ihv_type& r) const {
		for (int i = 0; i < 5; ++i)
			if (ihv[i] != r.ihv[i]) return false;
		return true;
	}
};

void analyze_indepsection_prob()
{
	vector< vector< vector<uint32> > > pathbitrelationsmatrix;
	mespace_to_pathbitrelationsmatrix(mainmespace, pathbitrelationsmatrix);
	sha1differentialpath path = maindiffpath;
	int tbegin = 16;
	for (; tbegin < 80; ++tbegin)
		if (0 == (path[tbegin].diff() | path[tbegin-1].diff() | path[tbegin-2].diff() | path[tbegin-3].diff() | path[tbegin-4].diff()))
			break;
	while (tbegin < 80) {
		int tend = tbegin+1;
		for (; tend < 80; ++tend)
			if (0 == (path[tend].diff() | path[tend-1].diff() | path[tend-2].diff() | path[tend-3].diff() | path[tend-4].diff()))
				break;
		if (0 != (path[tend].diff() | path[tend-1].diff() | path[tend-2].diff() | path[tend-3].diff() | path[tend-4].diff()))
			break;
		if (tend == tbegin+1) {
			tbegin = tend;
			continue;
		}
		cout << "Prob t=[" << tbegin << "-" << tend << "): \t" << flush;
#if 0
		tbegin = tend; cout << endl; continue;
#endif
		uint64 cnt = 0;
		uint32 okcnt = 0;
		while (okcnt < (1<<15)) {
			++cnt; if (hw(cnt)==1) cout << cnt << " " << flush;
			random_me(m, pathbitrelationsmatrix);
			for (int t = 0; t < 16; ++t)
				m2[t] = m[t] ^ path.getme(t).mask;
			for (int t = 16; t < 80; ++t)
				m2[t]=rotate_left(m2[t-3] ^ m2[t-8] ^ m2[t-14] ^ m2[t-16], 1);
			for (int t = tbegin-4; t <= tbegin; ++t)
				Q2[offset+t] = Q[offset+t] = xrng64();
			for (int t = tbegin; t < tend; ++t) {
				sha1_step(t, Q, m);
				sha1_step(t, Q2, m2);
			}
			bool ok = true;
			for (int t = tend-4; t <= tend; ++t)
				if (Q2[offset+t] != Q[offset+t]) {
					ok = false;
					break;
				}
			if (ok)
				if (hw(++okcnt)==1) cout << "[" << okcnt << "] " << flush;
		}
		cout << " prob=" << log(double(okcnt)/double(cnt))/log(2.0) << endl;

		tbegin = tend;
	}
	cout << "Prob t=[" << tbegin << "-" << 80 << "):" << endl;
	vector<ihv_type> target_dihvs(20);
	vector<ihv_type> dihvs(1<<28);
	progress_display pd(dihvs.size());
	for (unsigned i = 0; i < dihvs.size(); ++i,++pd) {
		random_me(m, pathbitrelationsmatrix);
		for (int t = 0; t < 16; ++t)
			m2[t] = m[t] ^ path.getme(t).mask;
		for (int t = 16; t < 80; ++t)
			m2[t]=rotate_left(m2[t-3] ^ m2[t-8] ^ m2[t-14] ^ m2[t-16], 1);
		for (int t = tbegin-4; t <= tbegin; ++t)
			Q2[offset+t] = Q[offset+t] = xrng64();
		for (int t = -4; t <= 0; ++t)
			Q2[offset+t] = Q[offset+t] = xrng64();
		for (int t = tbegin; t < 80; ++t) {
			sha1_step(t, Q, m);
			sha1_step(t, Q2, m2);
		}
		dihvs[i].ihv[0] = rotate_left(Q2[offset+80-4],30)-rotate_left(Q[offset+80-4],30);
		dihvs[i].ihv[1] = rotate_left(Q2[offset+80-3],30)-rotate_left(Q[offset+80-3],30);
		dihvs[i].ihv[2] = rotate_left(Q2[offset+80-2],30)-rotate_left(Q[offset+80-2],30);
		dihvs[i].ihv[3] = Q2[offset+80-1]-Q[offset+80-1];
		dihvs[i].ihv[4] = Q2[offset+80]-Q[offset+80];
	}
	sort(dihvs.begin(), dihvs.end());
	unsigned i = 0;
	vector<unsigned> bestcnts;
	while (i < dihvs.size()) {
		unsigned j = i+1;
		while (j < dihvs.size() && dihvs[j] == dihvs[i]) ++j;
		bestcnts.push_back(j-i);
		i = j;
	}
	cout << "Prob t=[" << tbegin << "-" << 80 << "): " << flush;
	sort(bestcnts.begin(),bestcnts.end());
	unsigned bestcnt = bestcnts[bestcnts.size()-1];
#if 1
	target_dihvs.clear();
	i = 0;
	while (i < dihvs.size()) {
		unsigned j = i+1;
		while (j < dihvs.size() && dihvs[j] == dihvs[i]) ++j;
		if ((j-i)*2 >= bestcnt) {
			cout << "\tprob=" << log(double(j-i)/double(dihvs.size()))/log(2.0) << ":\t";
			for (unsigned k = 0; k < 5; ++k)
				cout << naf(dihvs[i].ihv[k]) << " ";
			cout << endl;
			target_dihvs.push_back(dihvs[i]);
		}
		i = j;
	}
	{ vector<ihv_type> tmptmp(1); dihvs.swap(tmptmp); } // free memory
	uint64 cnt = 0, okcnt = 0;
	ihv_type dihv;
	while (true) {
		random_me(m, pathbitrelationsmatrix);
		for (int t = 0; t < 16; ++t)
			m2[t] = m[t] ^ path.getme(t).mask;
		for (int t = 16; t < 80; ++t)
			m2[t]=rotate_left(m2[t-3] ^ m2[t-8] ^ m2[t-14] ^ m2[t-16], 1);
		for (int t = tbegin-4; t <= tbegin; ++t)
			Q2[offset+t] = Q[offset+t] = xrng64();
		for (int t = -4; t <= 0; ++t)
			Q2[offset+t] = Q[offset+t] = xrng64();
		for (int t = tbegin; t < 80; ++t) {
			sha1_step(t, Q, m);
			sha1_step(t, Q2, m2);
		}
		dihv.ihv[0] = rotate_left(Q2[offset+80-4],30)-rotate_left(Q[offset+80-4],30);
		dihv.ihv[1] = rotate_left(Q2[offset+80-3],30)-rotate_left(Q[offset+80-3],30);
		dihv.ihv[2] = rotate_left(Q2[offset+80-2],30)-rotate_left(Q[offset+80-2],30);
		dihv.ihv[3] = Q2[offset+80-1]-Q[offset+80-1];
		dihv.ihv[4] = Q2[offset+80]-Q[offset+80];
		++cnt;
		for (unsigned k = 0; k < target_dihvs.size(); ++k)
			if (dihv == target_dihvs[k]) {
				++okcnt;
				break;
			}
		if (hw(cnt)+hw(cnt>>32)==1 && okcnt > 1) {
			cout << "[" << cnt << ":p=" << log(double(okcnt)/double(cnt))/log(2.0) << "]" << flush;
		}
	}
#else
	for (unsigned i = 0; i < bestcnts.size(); ++i)
		if (bestcnts[i]*2 >= bestcnt)
			cout << " prob=" << log(double(bestcnts[i])/double(dihvs.size()))/log(2.0) << flush;
#endif
	cout << endl;
	exit(0);
}