#include "ScaleOptimizer.h"
#include "FullSystem/FullSystem.h"
#include "FullSystem/HessianBlocks.h"
#include "FullSystem/Residuals.h"
#include "IOWrapper/ImageRW.h"
#include "OptimizationBackend/EnergyFunctionalStructs.h"
#include <algorithm>

#if !defined(__SSE3__) && !defined(__SSE2__) && !defined(__SSE1__)
#include "SSE2NEON.h"
#endif

namespace dso {

template <int b, typename T>
T *allocAligned(int size, std::vector<T *> &rawPtrVec) {
  const int padT = 1 + ((1 << b) / sizeof(T));
  T *ptr = new T[size + padT];
  rawPtrVec.push_back(ptr);
  T *alignedPtr = (T *)((((uintptr_t)(ptr + padT)) >> b) << b);
  return alignedPtr;
}

ScaleOptimizer::ScaleOptimizer(int w_, int h_, const Eigen::Matrix3f &K,
                               const SE3 &T_stereo_, float accept_th_) {
  // make coarse tracking templates.
  for (int lvl = 0; lvl < pyrLevelsUsed; lvl++) {
    int wl = w_ >> lvl;
    int hl = h_ >> lvl;

    idepth[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);
    weightSums[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);
    weightSums_bak[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);

    pc_u[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);
    pc_v[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);
    pc_idepth[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);
    pc_color[lvl] = allocAligned<4, float>(wl * hl, ptrToDelete);
  }

  // warped buffers
  buf_warped_rx1 = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_rx2 = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_rx3 = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_dx = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_dy = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_residual = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_weight = allocAligned<4, float>(w_ * h_, ptrToDelete);
  buf_warped_refColor = allocAligned<4, float>(w_ * h_, ptrToDelete);

  newFrame = 0;
  lastRef = 0;
  debugPlot = debugPrint = true;
  w[0] = h[0] = 0;
  refFrameID = -1;

  w[0] = w_;
  h[0] = h_;

  fx[0] = K(0, 0);
  fy[0] = K(1, 1);
  cx[0] = K(0, 2);
  cy[0] = K(1, 2);

  for (int level = 1; level < pyrLevelsUsed; ++level) {
    w[level] = w[0] >> level;
    h[level] = h[0] >> level;
    fx[level] = fx[level - 1] * 0.5;
    fy[level] = fy[level - 1] * 0.5;
    cx[level] = (cx[0] + 0.5) / ((int)1 << level) - 0.5;
    cy[level] = (cy[0] + 0.5) / ((int)1 << level) - 0.5;
  }

  accept_th = accept_th_;
  T_stereo = T_stereo_;
}

void ScaleOptimizer::makeK(CalibHessian *HCalib) {
  float fxl_orig = HCalib->fxl();
  float fyl_orig = HCalib->fyl();
  float cx0_orig = HCalib->cxl();
  float cy0_orig = HCalib->cyl();
  float cxl_orig = cx0_orig;
  float cyl_orig = cy0_orig;

  Mat33f K_orig_l;
  K_orig_l << fxl_orig, 0.0, cxl_orig, 0.0, fyl_orig, cyl_orig, 0.0, 0.0, 1.0;
  Ki_orig[0] = K_orig_l.inverse();

  for (int level = 1; level < pyrLevelsUsed; ++level) {
    fxl_orig = fxl_orig * 0.5;
    fyl_orig = fyl_orig * 0.5;
    cxl_orig = (cx0_orig + 0.5) / ((int)1 << level) - 0.5;
    cyl_orig = (cy0_orig + 0.5) / ((int)1 << level) - 0.5;
    K_orig_l << fxl_orig, 0.0, cxl_orig, 0.0, fyl_orig, cyl_orig, 0.0, 0.0, 1.0;
    Ki_orig[level] = K_orig_l.inverse();
  }
}

ScaleOptimizer::~ScaleOptimizer() {
  for (float *ptr : ptrToDelete)
    delete[] ptr;
  ptrToDelete.clear();
}

bool ScaleOptimizer::optimize(FrameHessian *newFrameHessian, float &scale,
                              int coarsestLvl, Vec5 minResForAbort,
                              IOWrap::Output3DWrapper *wrap) {
  debugPlot = setting_render_displayCoarseTrackingFull;
  // debugPlot = true;
  debugPrint = false;

  assert(coarsestLvl < 5 && coarsestLvl < pyrLevelsUsed);

  lastResiduals.setConstant(NAN);
  lastFlowIndicators.setConstant(1000);

  newFrame = newFrameHessian;
  int maxIterations[] = {10, 20, 50, 50, 50};
  float lambdaExtrapolationLimit = 0.001;

  float scale_current = scale;

  bool haveRepeated = false;
  bool haveAccepted = false;

  for (int lvl = coarsestLvl; lvl >= 0; lvl--) {
    float H;
    float b;
    float levelCutoffRepeat = 1;
    Vec6 resOld = calcRes(lvl, T_stereo, scale_current,
                          setting_coarseCutoffTH * levelCutoffRepeat);
    while (resOld[5] > 0.6 && levelCutoffRepeat < 50) {
      levelCutoffRepeat *= 2;
      resOld = calcRes(lvl, T_stereo, scale_current,
                       setting_coarseCutoffTH * levelCutoffRepeat);

      if (!setting_debugout_runquiet)
        printf("INCREASING cutoff to %f (ratio is %f)!\n",
               setting_coarseCutoffTH * levelCutoffRepeat, resOld[5]);
    }

    calcGSSSE(lvl, H, b, T_stereo, scale_current);

    float lambda = 0.01;

    if (debugPrint) {
      printf(
          "lvl%d, it %d (l=%f / %f) %s: %.3f->%.3f (%d -> %d) (inc = %f)! \n",
          lvl, -1, lambda, 1.0f, "INITIA", 0.0f, resOld[0] / resOld[1], 0,
          (int)resOld[1], 0.0f);
      std::cout << " Current scale " << scale_current << std::endl;
    }

    for (int iteration = 0; iteration < maxIterations[lvl]; iteration++) {
      float Hl = H;
      Hl *= (1 + lambda);
      float inc = -b / Hl;

      float extrapFac = 1;
      if (lambda < lambdaExtrapolationLimit)
        extrapFac = sqrt(sqrt(lambdaExtrapolationLimit / lambda));
      inc *= extrapFac;

      if (!std::isfinite(inc) || fabs(inc) > scale_current)
        inc = 0.0;

      float scale_new = scale_current + inc;

      Vec6 resNew = calcRes(lvl, T_stereo, scale_new,
                            setting_coarseCutoffTH * levelCutoffRepeat);

      bool accept = (resNew[0] / resNew[1]) < (resOld[0] / resOld[1]);

      if (debugPrint) {
        printf("lvl %d, it %d (l=%f / %f) %s: %.3f->%.3f (%d -> %d) (inc = "
               "%f)! \t",
               lvl, iteration, lambda, extrapFac,
               (accept ? "ACCEPT" : "REJECT"), resOld[0] / resOld[1],
               resNew[0] / resNew[1], (int)resOld[1], (int)resNew[1], inc);
        std::cout << " New scale " << scale_new << std::endl;
      }

      if (accept) {
        calcGSSSE(lvl, H, b, T_stereo, scale_new);
        resOld = resNew;
        scale_current = scale_new;
        lambda *= 0.5;
        haveAccepted = true;
      } else {
        lambda *= 4;
        if (lambda < lambdaExtrapolationLimit)
          lambda = lambdaExtrapolationLimit;
      }

      if (!(inc > 1e-3)) {
        if (debugPrint)
          printf("inc too small, break!\n");
        break;
      }
    }

    // set last residual for that level, as well as flow indicators.
    lastResiduals[lvl] = sqrtf((float)(resOld[0] / resOld[1]));
    lastFlowIndicators = resOld.segment<3>(2);
    if (lastResiduals[lvl] > 1.5 * minResForAbort[lvl])
      return false;

    if (levelCutoffRepeat > 1 && !haveRepeated) {
      lvl++;
      haveRepeated = true;
      printf("SCALE OPT. REPEAT LEVEL!\n");
    }
  }

  printf("Final scale: %f Final res: %f\n", scale_current, lastResiduals[0]);

  if (!haveAccepted || scale_current < 0 || !(lastResiduals[0] < accept_th)) {
    printf("Scale opt. rejected: coarsestLvl=%d, scale changed: %f -> %f\n",
           coarsestLvl, scale, scale_current);
    return false;
  }

  // set!
  scale = scale_current;

  return true;
}

void ScaleOptimizer::setCoarseTrackingRef(
    std::vector<FrameHessian *> frameHessians) {
  assert(frameHessians.size() > 0);
  lastRef = frameHessians.back();
  makeCoarseDepthL0(frameHessians);

  refFrameID = lastRef->shell->id;

  firstCoarseRMSE = -1;
}

void ScaleOptimizer::makeCoarseDepthL0(
    std::vector<FrameHessian *> frameHessians) {
  // make coarse tracking templates for latstRef.
  memset(idepth[0], 0, sizeof(float) * w[0] * h[0]);
  memset(weightSums[0], 0, sizeof(float) * w[0] * h[0]);

  for (FrameHessian *fh : frameHessians) {
    for (PointHessian *ph : fh->pointHessians) {
      if (ph->lastResiduals[0].first != 0 &&
          ph->lastResiduals[0].second == ResState::IN) {
        PointFrameResidual *r = ph->lastResiduals[0].first;
        assert(r->efResidual->isActive() && r->target == lastRef);
        int u = r->centerProjectedTo[0] + 0.5f;
        int v = r->centerProjectedTo[1] + 0.5f;
        float new_idepth = r->centerProjectedTo[2];
        float weight = sqrtf(1e-3 / (ph->efPoint->HdiF + 1e-12));

        idepth[0][u + w[0] * v] += new_idepth * weight;
        weightSums[0][u + w[0] * v] += weight;
      }
    }
  }

  for (int lvl = 1; lvl < pyrLevelsUsed; lvl++) {
    int lvlm1 = lvl - 1;
    int wl = w[lvl], hl = h[lvl], wlm1 = w[lvlm1];

    float *idepth_l = idepth[lvl];
    float *weightSums_l = weightSums[lvl];

    float *idepth_lm = idepth[lvlm1];
    float *weightSums_lm = weightSums[lvlm1];

    for (int y = 0; y < hl; y++)
      for (int x = 0; x < wl; x++) {
        int bidx = 2 * x + 2 * y * wlm1;
        idepth_l[x + y * wl] = idepth_lm[bidx] + idepth_lm[bidx + 1] +
                               idepth_lm[bidx + wlm1] +
                               idepth_lm[bidx + wlm1 + 1];

        weightSums_l[x + y * wl] =
            weightSums_lm[bidx] + weightSums_lm[bidx + 1] +
            weightSums_lm[bidx + wlm1] + weightSums_lm[bidx + wlm1 + 1];
      }
  }

  // dilate idepth by 1.
  for (int lvl = 0; lvl < 2; lvl++) {
    int numIts = 1;

    for (int it = 0; it < numIts; it++) {
      int wh = w[lvl] * h[lvl] - w[lvl];
      int wl = w[lvl];
      float *weightSumsl = weightSums[lvl];
      float *weightSumsl_bak = weightSums_bak[lvl];
      memcpy(weightSumsl_bak, weightSumsl, w[lvl] * h[lvl] * sizeof(float));
      float *idepthl =
          idepth[lvl]; // dotnt need to make a temp copy of depth, since I only
                       // read values with weightSumsl>0, and write ones with
                       // weightSumsl<=0.
      for (int i = w[lvl]; i < wh; i++) {
        if (weightSumsl_bak[i] <= 0) {
          float sum = 0, num = 0, numn = 0;
          if (weightSumsl_bak[i + 1 + wl] > 0) {
            sum += idepthl[i + 1 + wl];
            num += weightSumsl_bak[i + 1 + wl];
            numn++;
          }
          if (weightSumsl_bak[i - 1 - wl] > 0) {
            sum += idepthl[i - 1 - wl];
            num += weightSumsl_bak[i - 1 - wl];
            numn++;
          }
          if (weightSumsl_bak[i + wl - 1] > 0) {
            sum += idepthl[i + wl - 1];
            num += weightSumsl_bak[i + wl - 1];
            numn++;
          }
          if (weightSumsl_bak[i - wl + 1] > 0) {
            sum += idepthl[i - wl + 1];
            num += weightSumsl_bak[i - wl + 1];
            numn++;
          }
          if (numn > 0) {
            idepthl[i] = sum / numn;
            weightSumsl[i] = num / numn;
          }
        }
      }
    }
  }

  // dilate idepth by 1 (2 on lower levels).
  for (int lvl = 2; lvl < pyrLevelsUsed; lvl++) {
    int wh = w[lvl] * h[lvl] - w[lvl];
    int wl = w[lvl];
    float *weightSumsl = weightSums[lvl];
    float *weightSumsl_bak = weightSums_bak[lvl];
    memcpy(weightSumsl_bak, weightSumsl, w[lvl] * h[lvl] * sizeof(float));
    float *idepthl =
        idepth[lvl]; // dotnt need to make a temp copy of depth, since I only
                     // read values with weightSumsl>0, and write ones with
                     // weightSumsl<=0.
    for (int i = w[lvl]; i < wh; i++) {
      if (weightSumsl_bak[i] <= 0) {
        float sum = 0, num = 0, numn = 0;
        if (weightSumsl_bak[i + 1] > 0) {
          sum += idepthl[i + 1];
          num += weightSumsl_bak[i + 1];
          numn++;
        }
        if (weightSumsl_bak[i - 1] > 0) {
          sum += idepthl[i - 1];
          num += weightSumsl_bak[i - 1];
          numn++;
        }
        if (weightSumsl_bak[i + wl] > 0) {
          sum += idepthl[i + wl];
          num += weightSumsl_bak[i + wl];
          numn++;
        }
        if (weightSumsl_bak[i - wl] > 0) {
          sum += idepthl[i - wl];
          num += weightSumsl_bak[i - wl];
          numn++;
        }
        if (numn > 0) {
          idepthl[i] = sum / numn;
          weightSumsl[i] = num / numn;
        }
      }
    }
  }

  // normalize idepths and weights.
  for (int lvl = 0; lvl < pyrLevelsUsed; lvl++) {
    float *weightSumsl = weightSums[lvl];
    float *idepthl = idepth[lvl];
    Eigen::Vector3f *dIRefl = lastRef->dIp[lvl];

    int wl = w[lvl], hl = h[lvl];

    int lpc_n = 0;
    float *lpc_u = pc_u[lvl];
    float *lpc_v = pc_v[lvl];
    float *lpc_idepth = pc_idepth[lvl];
    float *lpc_color = pc_color[lvl];

    for (int y = 2; y < hl - 2; y++)
      for (int x = 2; x < wl - 2; x++) {
        int i = x + y * wl;

        if (weightSumsl[i] > 0) {
          idepthl[i] /= weightSumsl[i];
          lpc_u[lpc_n] = x;
          lpc_v[lpc_n] = y;
          lpc_idepth[lpc_n] = idepthl[i];
          lpc_color[lpc_n] = dIRefl[i][0];

          if (!std::isfinite(lpc_color[lpc_n]) || !(idepthl[i] > 0)) {
            idepthl[i] = -1;
            continue; // just skip if something is wrong.
          }
          lpc_n++;
        } else
          idepthl[i] = -1;

        weightSumsl[i] = 1;
      }

    pc_n[lvl] = lpc_n;
  }
}

void ScaleOptimizer::calcGSSSE(int lvl, float &H_out, float &b_out,
                               const SE3 &T_stereo, float scale) {
  acc.initialize();

  __m128 fxl = _mm_set1_ps(fx[lvl]);
  __m128 fyl = _mm_set1_ps(fy[lvl]);

  __m128 s = _mm_set1_ps(scale);
  __m128 tx = _mm_set1_ps(T_stereo.translation()[0]);
  __m128 ty = _mm_set1_ps(T_stereo.translation()[1]);
  __m128 tz = _mm_set1_ps(T_stereo.translation()[2]);

  __m128 one = _mm_set1_ps(1);

  int n = buf_warped_n;
  assert(n % 4 == 0);
  for (int i = 0; i < n; i += 4) {
    __m128 dxfx = _mm_mul_ps(_mm_load_ps(buf_warped_dx + i), fxl);
    __m128 dyfy = _mm_mul_ps(_mm_load_ps(buf_warped_dy + i), fyl);
    __m128 rx1 = _mm_load_ps(buf_warped_rx1 + i);
    __m128 rx2 = _mm_load_ps(buf_warped_rx2 + i);
    __m128 rx3 = _mm_load_ps(buf_warped_rx3 + i);

    __m128 deno_sqrt = _mm_add_ps(_mm_mul_ps(s, rx3), tz);
    __m128 deno = _mm_div_ps(one, _mm_mul_ps(deno_sqrt, deno_sqrt));

    __m128 xno = _mm_sub_ps(_mm_mul_ps(rx1, tz), _mm_mul_ps(rx3, tx));
    __m128 yno = _mm_sub_ps(_mm_mul_ps(rx2, tz), _mm_mul_ps(rx3, ty));

    acc.updateSSE_oneed(_mm_add_ps(_mm_mul_ps(dxfx, _mm_mul_ps(deno, xno)),
                                   _mm_mul_ps(dyfy, _mm_mul_ps(deno, yno))),
                        _mm_load_ps(buf_warped_residual + i),
                        _mm_load_ps(buf_warped_weight + i));
  }

  acc.finish();
  H_out = acc.H(0, 0) * (1.0f / n);
  b_out = acc.H(0, 1) * (1.0f / n);
}

Vec6 ScaleOptimizer::calcRes(int lvl, const SE3 &T_stereo, float scale,
                             float cutoffTH) {
  float E = 0;
  int numTermsInE = 0;
  int numTermsInWarped = 0;
  int numSaturated = 0;

  int wl = w[lvl];
  int hl = h[lvl];
  Eigen::Vector3f *dINewl = newFrame->dIp[lvl];
  float fxl = fx[lvl];
  float fyl = fy[lvl];
  float cxl = cx[lvl];
  float cyl = cy[lvl];

  Mat33f RKi_orig = (T_stereo.rotationMatrix().cast<float>() * Ki_orig[lvl]);
  Vec3f t = (T_stereo.translation()).cast<float>();

  float sumSquaredShiftT = 0;
  float sumSquaredShiftRT = 0;
  float sumSquaredShiftNum = 0;

  float maxEnergy =
      2 * setting_huberTH * cutoffTH -
      setting_huberTH * setting_huberTH; // energy for r=setting_coarseCutoffTH.

  MinimalImageB3 *resImage = 0;
  MinimalImageB3 *projImage = 0;
  if (debugPlot) {
    resImage = new MinimalImageB3(wl, hl);
    resImage->setConst(Vec3b(255, 255, 255));

    projImage = new MinimalImageB3(wl, hl);
    projImage->setBlack();
    for (int i = 0; i < h[lvl] * w[lvl]; i++) {
      int c = newFrame->dIp[lvl][i][0] * 0.9f;
      if (c > 255)
        c = 255;
      projImage->at(i) = Vec3b(c, c, c);
    }
  }

  int nl = pc_n[lvl];
  float *lpc_u = pc_u[lvl];
  float *lpc_v = pc_v[lvl];
  float *lpc_idepth = pc_idepth[lvl];
  float *lpc_color = pc_color[lvl];

  for (int i = 0; i < nl; i++) {
    float id = lpc_idepth[i];
    float x = lpc_u[i];
    float y = lpc_v[i];

    Vec3f pt = scale * RKi_orig * Vec3f(x, y, 1) + t * id;
    float u = pt[0] / pt[2];
    float v = pt[1] / pt[2];
    float Ku = fxl * u + cxl;
    float Kv = fyl * v + cyl;
    float new_idepth = id / pt[2];

    Vec3f rx = RKi_orig * Vec3f(x, y, 1) / id;

    if (lvl == 0 && i % 32 == 0) {
      // translation only (positive)
      Vec3f ptT = scale * Ki_orig[lvl] * Vec3f(x, y, 1) + t * id;
      float uT = ptT[0] / ptT[2];
      float vT = ptT[1] / ptT[2];
      float KuT = fxl * uT + cxl;
      float KvT = fyl * vT + cyl;

      // translation only (negative)
      Vec3f ptT2 = scale * Ki_orig[lvl] * Vec3f(x, y, 1) - t * id;
      float uT2 = ptT2[0] / ptT2[2];
      float vT2 = ptT2[1] / ptT2[2];
      float KuT2 = fxl * uT2 + cxl;
      float KvT2 = fyl * vT2 + cyl;

      // translation and rotation (negative)
      Vec3f pt3 = scale * RKi_orig * Vec3f(x, y, 1) - t * id;
      float u3 = pt3[0] / pt3[2];
      float v3 = pt3[1] / pt3[2];
      float Ku3 = fxl * u3 + cxl;
      float Kv3 = fyl * v3 + cyl;

      // translation and rotation (positive)
      // already have it.

      sumSquaredShiftT += (KuT - x) * (KuT - x) + (KvT - y) * (KvT - y);
      sumSquaredShiftT += (KuT2 - x) * (KuT2 - x) + (KvT2 - y) * (KvT2 - y);
      sumSquaredShiftRT += (Ku - x) * (Ku - x) + (Kv - y) * (Kv - y);
      sumSquaredShiftRT += (Ku3 - x) * (Ku3 - x) + (Kv3 - y) * (Kv3 - y);
      sumSquaredShiftNum += 2;
    }

    if (!(Ku > 2 && Kv > 2 && Ku < wl - 3 && Kv < hl - 3 && new_idepth > 0))
      continue;

    float refColor = lpc_color[i];
    Vec3f hitColor = getInterpolatedElement33(dINewl, Ku, Kv, wl);
    if (!std::isfinite((float)hitColor[0]))
      continue;
    float residual = hitColor[0] - refColor;
    float hw =
        fabs(residual) < setting_huberTH ? 1 : setting_huberTH / fabs(residual);

    // if(debugPlot) resImage->setPixel4(lpc_u[i], lpc_v[i], Vec3b(0,0,255));
    if (debugPlot)
      projImage->setPixel4(Ku, Kv, Vec3b(0, 0, 255));

    if (fabs(residual) > cutoffTH) {
      if (debugPlot)
        resImage->setPixel4(lpc_u[i], lpc_v[i], Vec3b(0, 0, 255));
      E += maxEnergy;
      numTermsInE++;
      numSaturated++;
    } else {
      if (debugPlot)
        resImage->setPixel4(
            lpc_u[i], lpc_v[i],
            Vec3b(residual + 128, residual + 128, residual + 128));
      E += hw * residual * residual * (2 - hw);
      numTermsInE++;

      buf_warped_rx1[numTermsInWarped] = rx[0];
      buf_warped_rx2[numTermsInWarped] = rx[1];
      buf_warped_rx3[numTermsInWarped] = rx[2];
      buf_warped_dx[numTermsInWarped] = hitColor[1];
      buf_warped_dy[numTermsInWarped] = hitColor[2];
      buf_warped_residual[numTermsInWarped] = residual;
      buf_warped_weight[numTermsInWarped] = hw;
      buf_warped_refColor[numTermsInWarped] = lpc_color[i];
      numTermsInWarped++;
    }
  }

  while (numTermsInWarped % 4 != 0) {
    buf_warped_rx1[numTermsInWarped] = 0;
    buf_warped_rx2[numTermsInWarped] = 0;
    buf_warped_rx3[numTermsInWarped] = 0;
    buf_warped_dx[numTermsInWarped] = 0;
    buf_warped_dy[numTermsInWarped] = 0;
    buf_warped_residual[numTermsInWarped] = 0;
    buf_warped_weight[numTermsInWarped] = 0;
    buf_warped_refColor[numTermsInWarped] = 0;
    numTermsInWarped++;
  }
  buf_warped_n = numTermsInWarped;

  if (debugPlot) {
    IOWrap::displayImage("Scale RES", resImage, false);
    IOWrap::displayImage("Proj", projImage, false);
    IOWrap::waitKey(0);
    delete resImage;
    delete projImage;
  }

  Vec6 rs;
  rs[0] = E;
  rs[1] = numTermsInE;
  rs[2] = sumSquaredShiftT / (sumSquaredShiftNum + 0.1);
  rs[3] = 0;
  rs[4] = sumSquaredShiftRT / (sumSquaredShiftNum + 0.1);
  rs[5] = numSaturated / (float)numTermsInE;

  return rs;
}

} // namespace dso
