// HardwareSortingMethods.cc
// Authors: R. Alex Barbieri
//          Ben Kreis
//
// This file should contain the C++ equivalents of the sorting
// algorithms used in Hardware. Most C++ methods originally written by
// Ben Kreis.

#include <iostream>
#include "L1Trigger/L1TCalorimeter/interface/HardwareSortingMethods.h"
#include "FWCore/MessageLogger/interface/MessageLogger.h"

namespace {
  constexpr bool verbose = false;

  constexpr int fw_to_gt_phi_map[] = {4, 3, 2, 1, 0, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5};
  constexpr int gt_to_fw_phi_map[] = {4, 3, 2, 1, 0, 17, 16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5};
}  // namespace

namespace l1t {
  unsigned int pack15bits(int pt, int eta, int phi) {
    return (((pt & 0x3f)) + ((eta & 0xf) << 6) + ((phi & 0x1f) << 10));
  }

  unsigned int pack16bits(int pt, int eta, int phi) {
    return (0x8000 + ((pt & 0x3f)) + ((eta & 0xf) << 6) + ((phi & 0x1f) << 10));
  }

  unsigned int pack16bitsEgammaSpecial(int pt, int eta, int phi) {
    return (0x8000 + ((pt & 0x3f) << 9) + ((eta & 0xf)) + ((phi & 0x1f) << 4));
  }
}  // namespace l1t

void print2DVector(std::vector<std::vector<l1t::L1Candidate> > const& myVector) {
  int nrows = myVector.size();
  int ncols = myVector[0].size();
  std::cout << std::endl;
  std::cout << "rows: " << nrows << std::endl;
  std::cout << "cols: " << ncols << std::endl;

  for (int r = 0; r < nrows; r++) {
    for (int c = 0; c < ncols; c++) {
      std::cout << std::setw(5) << myVector[r][c].hwPt() << ' ';
    }
    std::cout << std::endl;
  }
}

std::vector<l1t::L1Candidate> sort_array(std::vector<l1t::L1Candidate> const& inputArray) {
  std::vector<l1t::L1Candidate> outputArray(inputArray.size());
  for (unsigned int i = 0; i < inputArray.size(); i++) {
    int rank = 0;
    for (unsigned int j = 0; j < inputArray.size(); j++) {
      if ((inputArray[i].hwPt() > inputArray[j].hwPt()) || ((inputArray[i].hwPt() == inputArray[j].hwPt()) && i < j))
        rank++;
    }  //j
    outputArray[outputArray.size() - 1 - rank] = inputArray[i];
  }  //i
  return outputArray;
}

std::vector<std::vector<l1t::L1Candidate> > presort(std::vector<std::vector<l1t::L1Candidate> > const& energies,
                                                    int rows,
                                                    int cols) {
  int row_block_length = energies.size() / cols;
  if (energies.size() % cols != 0)
    row_block_length++;

  l1t::L1Candidate dummyJet;
  dummyJet.setHwPt(0);
  dummyJet.setHwPhi(99);
  dummyJet.setHwEta(99);
  dummyJet.setHwQual(0x10);
  std::vector<std::vector<l1t::L1Candidate> > sorted_energies(rows, std::vector<l1t::L1Candidate>(cols, dummyJet));
  if (verbose)
    print2DVector(sorted_energies);

  unsigned int row = 0, col = 0;
  std::vector<l1t::L1Candidate> energy_feeder(cols, dummyJet);
  std::vector<l1t::L1Candidate> energy_result(cols, dummyJet);
  for (int r = 0; r < rows; r++) {
    for (int c = 0; c < cols; c++) {
      row = (r % row_block_length) * cols + c;  //row goes up to 19 and we pad with zeros

      if (row < energies.size()) {
        energy_feeder[c] = energies[row][col];
      } else {
        energy_feeder[c] = dummyJet;
      }

    }  //c

    energy_result = sort_array(energy_feeder);  //sort!

    sorted_energies[r] = energy_result;

    if (r % row_block_length == row_block_length - 1)
      col++;

  }  //r
  if (verbose)
    print2DVector(sorted_energies);

  return sorted_energies;
}

std::vector<std::vector<l1t::L1Candidate> > extract_sub_jet_energy_position_matrix(
    std::vector<std::vector<l1t::L1Candidate> > const& input_matrix,
    unsigned int row_i,
    unsigned int row_f,
    unsigned int col_i,
    unsigned int col_f) {
  std::vector<std::vector<l1t::L1Candidate> > output_matrix(row_f - row_i + 1,
                                                            std::vector<l1t::L1Candidate>(col_f - col_i + 1));
  l1t::L1Candidate dummyJet;
  dummyJet.setHwPt(0);
  dummyJet.setHwPhi(99);
  dummyJet.setHwEta(99);
  dummyJet.setHwQual(0x10);
  for (unsigned int i = 0; i < row_f - row_i + 1; i++) {
    for (unsigned int j = 0; j < col_f - col_i + 1; j++) {
      if (row_i + i > input_matrix.size() - 1)
        output_matrix[i][j] = dummyJet;
      else
        output_matrix[i][j] = input_matrix[row_i + i][col_i + j];
    }  //j
  }    //i
  return output_matrix;
}

std::vector<std::vector<l1t::L1Candidate> > sort_matrix_rows(
    std::vector<std::vector<l1t::L1Candidate> > const& input_matrix) {
  std::vector<std::vector<l1t::L1Candidate> > output_matrix(input_matrix.size(),
                                                            std::vector<l1t::L1Candidate>(input_matrix[0].size()));

  for (unsigned int i = 0; i < input_matrix.size(); i++) {
    int rank = 0;
    for (unsigned int j = 0; j < input_matrix.size(); j++) {
      if ((input_matrix[i][0].hwPt() > input_matrix[j][0].hwPt()) ||
          ((input_matrix[i][0].hwPt() == input_matrix[j][0].hwPt()) && i < j))
        rank++;
    }  //j
    output_matrix[input_matrix.size() - 1 - rank] = input_matrix[i];
  }  //i

  return output_matrix;
}

std::vector<std::vector<l1t::L1Candidate> > sort_by_row_in_groups(
    std::vector<std::vector<l1t::L1Candidate> > const& input_matrix, int group_size) {
  int n_groups =
      input_matrix.size() / group_size +
      (1 - input_matrix.size() / group_size * group_size / input_matrix.size());  //constants must make this an integer
  //std::vector<std::vector<l1t::L1Candidate> > output_matrix(input_matrix.size()+(input_matrix.size() % group_size), std::vector<l1t::L1Candidate> (input_matrix[0].size()));
  std::vector<std::vector<l1t::L1Candidate> > output_matrix(
      input_matrix.size() +
          (group_size * (1 - ((input_matrix.size() / group_size) * group_size) / input_matrix.size())) -
          (input_matrix.size() % group_size),
      std::vector<l1t::L1Candidate>(input_matrix[0].size()));

  for (int g = 0; g < n_groups; g++) {
    std::vector<std::vector<l1t::L1Candidate> > small_output_matrix = extract_sub_jet_energy_position_matrix(
        input_matrix, g * group_size, (g + 1) * group_size - 1, 0, input_matrix[0].size() - 1);
    small_output_matrix = sort_matrix_rows(small_output_matrix);

    for (unsigned int i = 0; i < small_output_matrix.size(); i++) {
      output_matrix[g * group_size + i] = small_output_matrix[i];
    }
  }

  if (verbose)
    print2DVector(output_matrix);
  return output_matrix;
}

std::vector<l1t::L1Candidate> array_from_row_sorted_matrix(
    std::vector<std::vector<l1t::L1Candidate> > const& input_matrix, unsigned int n_keep) {
  std::vector<l1t::L1Candidate> output_array(n_keep * (n_keep + 1) / 2);
  unsigned int max_row = n_keep - 1;
  unsigned int max_col = n_keep - 1;

  //compute size
  if (input_matrix.size() < n_keep)
    max_row = input_matrix.size() - 1;
  if (input_matrix[0].size() < n_keep)
    max_col = input_matrix[0].size() - 1;

  unsigned int array_position = 0;
  for (unsigned int i = 0; i <= max_row; i++) {
    for (unsigned int j = 0; j <= max_col - i; j++) {
      //cout << input_matrix[i][j].hwPt() << endl;
      output_array[array_position] = input_matrix[i][j];
      array_position++;
    }  //j
  }    //i

  //fill rest with zeros
  l1t::L1Candidate dummyJet;
  dummyJet.setHwPt(0);
  dummyJet.setHwPhi(99);
  dummyJet.setHwEta(99);
  dummyJet.setHwQual(0x10);
  for (unsigned int k = array_position; k < output_array.size(); k++) {
    output_array[k] = dummyJet;
  }

  //printVector(output_array);
  return output_array;
}

std::vector<std::vector<l1t::L1Candidate> > super_sort_matrix_rows(
    std::vector<std::vector<l1t::L1Candidate> > const& input_matrix, unsigned int group_size, unsigned int n_keep) {
  unsigned int n_groups =
      input_matrix.size() / group_size +
      (1 - input_matrix.size() / group_size * group_size / input_matrix.size());  //constants must make this an integer
  std::vector<std::vector<l1t::L1Candidate> > output_matrix(n_groups, std::vector<l1t::L1Candidate>(n_keep));

  for (unsigned int g = 0; g < n_groups; g++) {
    std::vector<std::vector<l1t::L1Candidate> > small_output_matrix = extract_sub_jet_energy_position_matrix(
        input_matrix, g * group_size, (g + 1) * group_size - 1, 0, input_matrix[0].size() - 1);
    std::vector<l1t::L1Candidate> unsorted_array = array_from_row_sorted_matrix(small_output_matrix, n_keep);
    std::vector<l1t::L1Candidate> unsorted_array_without_largest(unsorted_array.size() -
                                                                 1);  //we know first element is the biggest
    for (unsigned int i = 0; i < unsorted_array.size() - 1; i++) {
      unsorted_array_without_largest[i] = unsorted_array[1 + i];
    }
    std::vector<l1t::L1Candidate> sorted_array_without_largest = sort_array(unsorted_array_without_largest);

    std::vector<l1t::L1Candidate> sorted_array(n_keep);
    sorted_array[0] = unsorted_array[0];
    for (unsigned int i = 0; i < n_keep - 1; i++) {
      sorted_array[1 + i] = sorted_array_without_largest[i];
    }

    output_matrix[g] = sorted_array;
  }  //g

  if (verbose)
    print2DVector(output_matrix);
  return output_matrix;
}

std::vector<std::vector<l1t::L1Candidate> > presort_egamma(std::vector<l1t::L1Candidate> const& input_egamma,
                                                           int rows,
                                                           int cols) {
  int row_block_length = input_egamma.size() / cols;
  if (input_egamma.size() % cols != 0)
    row_block_length++;

  //Initialize output
  l1t::L1Candidate dummyJet;
  dummyJet.setHwPt(0);
  dummyJet.setHwPhi(99);
  dummyJet.setHwEta(99);
  dummyJet.setHwQual(0x10);
  std::vector<std::vector<l1t::L1Candidate> > sorted_energies(rows, std::vector<l1t::L1Candidate>(cols, dummyJet));
  if (verbose)
    print2DVector(sorted_energies);

  unsigned int row = 0;
  std::vector<l1t::L1Candidate> energy_feeder(cols, dummyJet);
  std::vector<l1t::L1Candidate> energy_result(cols, dummyJet);
  for (int r = 0; r < rows; r++) {
    for (int c = 0; c < cols; c++) {
      row = (r % row_block_length) * cols + c;  //row goes up to 19 and we pad with zeros

      if (row < input_egamma.size()) {
        energy_feeder[c] = input_egamma[row];
      } else {
        energy_feeder[c] = dummyJet;
      }

    }  //c

    energy_result = sort_array(energy_feeder);  //sort!

    sorted_energies[r] = energy_result;
  }  //r
  if (verbose)
    print2DVector(sorted_energies);

  return sorted_energies;
}

namespace l1t {
  void SortJets(std::vector<l1t::Jet>* input, std::vector<l1t::Jet>* output) {
    //verbose = true;
    const int CENTRAL_ETA_SLICES = 14;
    const int N_PHI_GROUPS = 5;
    const int N_PRESORTED_ROWS_CENTRAL = CENTRAL_ETA_SLICES * N_PHI_GROUPS;
    const int PRESORT_DEPTH = 4;
    const int N_KEEP_CENTRAL = 4;
    const int N_ETA_GROUP_SIZE_CENTRAL = 4;
    const int N_ETA_GROUPS_CENTRAL = 4;

    const int HFM_ETA_SLICES = 4;
    const int HFP_ETA_SLICES = 4;
    const int N_PRESORTED_ROWS_HFM = HFM_ETA_SLICES * N_PHI_GROUPS;
    const int N_PRESORTED_ROWS_HFP = HFP_ETA_SLICES * N_PHI_GROUPS;
    const int N_KEEP_FORWARD = 4;

    const int cen_nrows = 18;
    const int cen_ncols = 14;
    const int hfm_nrows = 18, hfp_nrows = 18;
    const int hfm_ncols = 4, hfp_ncols = 4;

    std::vector<std::vector<l1t::L1Candidate> > cen_input_energy(cen_nrows, std::vector<l1t::L1Candidate>(cen_ncols));
    std::vector<std::vector<l1t::L1Candidate> > hfm_input_energy(hfm_nrows, std::vector<l1t::L1Candidate>(hfm_ncols));
    std::vector<std::vector<l1t::L1Candidate> > hfp_input_energy(hfp_nrows, std::vector<l1t::L1Candidate>(hfp_ncols));

    for (std::vector<l1t::Jet>::const_iterator injet = input->begin(); injet != input->end(); ++injet) {
      if (injet->hwEta() >= 4 && injet->hwEta() <= 17) {
        unsigned int myrow = gt_to_fw_phi_map[injet->hwPhi()];
        unsigned int mycol = injet->hwEta() - 4;  //hardcoding is bad
        cen_input_energy[myrow][mycol] = *injet;
      } else if (injet->hwEta() < 4) {
        unsigned int myrow = gt_to_fw_phi_map[injet->hwPhi()];
        unsigned int mycol = injet->hwEta();  //hardcoding is bad
        hfm_input_energy[myrow][mycol] = *injet;
      } else if (injet->hwEta() > 17) {
        unsigned int myrow = gt_to_fw_phi_map[injet->hwPhi()];
        unsigned int mycol = injet->hwEta() - 18;  //hardcoding is bad
        hfp_input_energy[myrow][mycol] = *injet;
      } else
        edm::LogError("HardwareJetSort") << "Region out of bounds: " << injet->hwEta();
    }

    for (int i = 0; i < cen_nrows; ++i)
      for (int j = 0; j < cen_ncols; ++j) {
        if (cen_input_energy[i][j].hwPt() == 0) {
          cen_input_energy[i][j].setHwPhi(fw_to_gt_phi_map[i]);
          cen_input_energy[i][j].setHwEta(4 + j);
        }
      }

    for (int i = 0; i < hfm_nrows; ++i)
      for (int j = 0; j < hfm_ncols; ++j) {
        if (hfm_input_energy[i][j].hwPt() == 0) {
          hfm_input_energy[i][j].setHwPhi(fw_to_gt_phi_map[i]);
          hfm_input_energy[i][j].setHwEta(j);
          hfm_input_energy[i][j].setHwQual(2);
        }
      }

    for (int i = 0; i < hfp_nrows; ++i)
      for (int j = 0; j < hfp_ncols; ++j) {
        if (hfp_input_energy[i][j].hwPt() == 0) {
          hfp_input_energy[i][j].setHwPhi(fw_to_gt_phi_map[i]);
          hfp_input_energy[i][j].setHwEta(j + 18);
          hfp_input_energy[i][j].setHwQual(2);
        }
      }

    //Each CLK is one clock

    //CLK 1
    std::vector<std::vector<l1t::L1Candidate> > presorted_energies_matrix_sig =
        presort(cen_input_energy, N_PRESORTED_ROWS_CENTRAL, PRESORT_DEPTH);
    std::vector<std::vector<l1t::L1Candidate> > hfm_presorted_energies_matrix_sig =
        presort(hfm_input_energy, N_PRESORTED_ROWS_HFM, PRESORT_DEPTH);
    std::vector<std::vector<l1t::L1Candidate> > hfp_presorted_energies_matrix_sig =
        presort(hfp_input_energy, N_PRESORTED_ROWS_HFP, PRESORT_DEPTH);

    //CLK 2
    std::vector<std::vector<l1t::L1Candidate> > row_presorted_energies_matrix_sig =
        sort_by_row_in_groups(presorted_energies_matrix_sig, N_PHI_GROUPS);
    std::vector<std::vector<l1t::L1Candidate> > hfm_row_presorted_energies_matrix_sig =
        sort_by_row_in_groups(hfm_presorted_energies_matrix_sig, N_PHI_GROUPS);
    std::vector<std::vector<l1t::L1Candidate> > hfp_row_presorted_energies_matrix_sig =
        sort_by_row_in_groups(hfp_presorted_energies_matrix_sig, N_PHI_GROUPS);

    //CLK 3
    std::vector<std::vector<l1t::L1Candidate> > sorted_eta_slices_energies_matrix_sig =
        super_sort_matrix_rows(row_presorted_energies_matrix_sig, N_PHI_GROUPS, N_KEEP_CENTRAL);
    std::vector<std::vector<l1t::L1Candidate> > hfm_sorted_eta_slices_energies_matrix_sig =
        super_sort_matrix_rows(hfm_row_presorted_energies_matrix_sig, N_PHI_GROUPS, N_KEEP_FORWARD);
    std::vector<std::vector<l1t::L1Candidate> > hfp_sorted_eta_slices_energies_matrix_sig =
        super_sort_matrix_rows(hfp_row_presorted_energies_matrix_sig, N_PHI_GROUPS, N_KEEP_FORWARD);

    //CLK 4
    std::vector<std::vector<l1t::L1Candidate> > row_presorted_eta_slices_energies_matrix_sig =
        sort_by_row_in_groups(sorted_eta_slices_energies_matrix_sig, N_ETA_GROUP_SIZE_CENTRAL);
    std::vector<std::vector<l1t::L1Candidate> > hfm_row_presorted_eta_slices_energies_matrix_sig =
        sort_by_row_in_groups(hfm_sorted_eta_slices_energies_matrix_sig, HFM_ETA_SLICES);
    std::vector<std::vector<l1t::L1Candidate> > hfp_row_presorted_eta_slices_energies_matrix_sig =
        sort_by_row_in_groups(hfp_sorted_eta_slices_energies_matrix_sig, HFP_ETA_SLICES);

    //CLK 5
    std::vector<std::vector<l1t::L1Candidate> > sorted_eta_groups_energies_matrix_sig =
        super_sort_matrix_rows(row_presorted_eta_slices_energies_matrix_sig, N_ETA_GROUP_SIZE_CENTRAL, N_KEEP_CENTRAL);
    std::vector<std::vector<l1t::L1Candidate> > hfm_sorted_final_energies_matrix_sig =
        super_sort_matrix_rows(hfm_row_presorted_eta_slices_energies_matrix_sig, HFM_ETA_SLICES, N_KEEP_FORWARD);
    std::vector<std::vector<l1t::L1Candidate> > hfp_sorted_final_energies_matrix_sig =
        super_sort_matrix_rows(hfp_row_presorted_eta_slices_energies_matrix_sig, HFP_ETA_SLICES, N_KEEP_FORWARD);

    //CLK 6
    std::vector<std::vector<l1t::L1Candidate> > row_presorted_eta_groups_energies_matrix_sig =
        sort_by_row_in_groups(sorted_eta_groups_energies_matrix_sig, N_ETA_GROUPS_CENTRAL);
    std::vector<std::vector<l1t::L1Candidate> > hf_merged_plus_minus_forward_energies_matrix_sig(
        2, std::vector<l1t::L1Candidate>(N_KEEP_FORWARD));
    hf_merged_plus_minus_forward_energies_matrix_sig[0] = hfm_sorted_final_energies_matrix_sig[0];
    hf_merged_plus_minus_forward_energies_matrix_sig[1] = hfp_sorted_final_energies_matrix_sig[0];
    std::vector<std::vector<l1t::L1Candidate> > hf_row_presorted_merged_plus_minus_forward_energies_matrix_sig =
        sort_by_row_in_groups(hf_merged_plus_minus_forward_energies_matrix_sig, 2);

    //CLK 7
    std::vector<std::vector<l1t::L1Candidate> > sorted_final_energies_matrix_sig =
        super_sort_matrix_rows(row_presorted_eta_groups_energies_matrix_sig, N_ETA_GROUPS_CENTRAL, N_KEEP_CENTRAL);
    std::vector<std::vector<l1t::L1Candidate> > hf_sorted_final_merged_plus_minus_forward_energies_matrix_sig =
        super_sort_matrix_rows(hf_row_presorted_merged_plus_minus_forward_energies_matrix_sig, 2, N_KEEP_FORWARD);

    for (unsigned int i = 0; i < 4; ++i) {
      auto const& tmp = sorted_final_energies_matrix_sig[0][i];
      output->emplace_back(tmp.p4(), tmp.hwPt(), tmp.hwEta(), tmp.hwPhi(), tmp.hwQual());
    }
    for (unsigned int i = 0; i < 4; ++i) {
      auto const& tmp = hf_sorted_final_merged_plus_minus_forward_energies_matrix_sig[0][i];
      output->emplace_back(tmp.p4(), tmp.hwPt(), tmp.hwEta(), tmp.hwPhi(), tmp.hwQual() | 2);
    }
    //verbose = false;
  }

  void SortEGammas(std::vector<l1t::EGamma>* input, std::vector<l1t::EGamma>* output) {
    //Initialize
    const int FIBER_PAIRS = 18;
    const int N_INPUT_EGAMMAS = 4;
    const int N_PRESORTED_ROWS_EGAMMA = 36;
    const int PRESORT_DEPTH = 4;
    const int N_EGAMMA_FIRST_GROUP_SIZE = 6;
    const int N_EGAMMA_SECOND_GROUP_SIZE = 6;
    const int N_EGAMMA_FIRST_GROUPS = 6;
    const int N_KEEP_EGAMMA = 4;

    //Input
    //Each egamma: RCT isolation, RCT order, phi index, eta index

    vector<l1t::L1Candidate> iso_egamma_array_p,
        iso_egamma_array_m;  //reusing objects.  should probably rename to something like "object"
    vector<l1t::L1Candidate> noniso_egamma_array_p, noniso_egamma_array_m;

    for (int k = 0; k < 2 * N_INPUT_EGAMMAS * FIBER_PAIRS; k++) {
      l1t::L1Candidate dummyJet;
      dummyJet.setHwPt(0);
      dummyJet.setHwEta(99);
      dummyJet.setHwPhi(99);
      dummyJet.setHwQual(0x10);
      if (k < N_INPUT_EGAMMAS * FIBER_PAIRS) {
        iso_egamma_array_p.push_back(dummyJet);
        noniso_egamma_array_p.push_back(dummyJet);
      } else {
        iso_egamma_array_m.push_back(dummyJet);
        noniso_egamma_array_m.push_back(dummyJet);
      }
    }

    for (std::vector<l1t::EGamma>::const_iterator ineg = input->begin(); ineg != input->end(); ++ineg) {
      int fiberNum = (int)floor(gt_to_fw_phi_map[ineg->hwPhi()] / 2);
      int index = ineg->hwQual();
      bool iso = ineg->hwIso();
      bool minus = (ineg->hwEta() < 11);

      // while waiting for firmware LUT, set all iso to true
      //iso = true;

      if (iso && minus)
        iso_egamma_array_m[8 * fiberNum + index] = *ineg;
      else if (iso && !minus)
        iso_egamma_array_p[8 * fiberNum + index] = *ineg;
      else if (!iso && minus)
        noniso_egamma_array_m[8 * fiberNum + index] = *ineg;
      else if (!iso && !minus)
        noniso_egamma_array_p[8 * fiberNum + index] = *ineg;
    }

    // std::cout << "iso_egamma_array_m" << std::endl;
    // for(int i = 0; i < (int)iso_egamma_array_m.size(); ++i)
    // {
    //   std::cout << iso_egamma_array_m[i].hwPt() << " "
    // 	   << iso_egamma_array_m[i].hwEta() << " "
    // 	   << iso_egamma_array_m[i].hwPhi() << std::endl;
    // }

    // std::cout << "iso_egamma_array_p" << std::endl;
    // for(int i = 0; i < (int)iso_egamma_array_p.size(); ++i)
    // {
    //   std::cout << iso_egamma_array_p[i].hwPt() << " "
    // 	   << iso_egamma_array_p[i].hwEta() << " "
    // 	   << iso_egamma_array_p[i].hwPhi() << std::endl;
    // }

    //verbose = true;
    //1
    std::vector<std::vector<l1t::L1Candidate> > presorted_iso_matrix_sig_p =
        presort_egamma(iso_egamma_array_p, N_PRESORTED_ROWS_EGAMMA / 2, PRESORT_DEPTH);
    std::vector<std::vector<l1t::L1Candidate> > presorted_iso_matrix_sig_m =
        presort_egamma(iso_egamma_array_m, N_PRESORTED_ROWS_EGAMMA / 2, PRESORT_DEPTH);
    std::vector<std::vector<l1t::L1Candidate> > presorted_non_iso_matrix_sig_p =
        presort_egamma(noniso_egamma_array_p, N_PRESORTED_ROWS_EGAMMA / 2, PRESORT_DEPTH);
    std::vector<std::vector<l1t::L1Candidate> > presorted_non_iso_matrix_sig_m =
        presort_egamma(noniso_egamma_array_m, N_PRESORTED_ROWS_EGAMMA / 2, PRESORT_DEPTH);

    //2
    std::vector<std::vector<l1t::L1Candidate> > iso_row_presorted_energies_matrix_sig_p =
        sort_by_row_in_groups(presorted_iso_matrix_sig_p, N_EGAMMA_FIRST_GROUP_SIZE);
    std::vector<std::vector<l1t::L1Candidate> > iso_row_presorted_energies_matrix_sig_m =
        sort_by_row_in_groups(presorted_iso_matrix_sig_m, N_EGAMMA_FIRST_GROUP_SIZE);
    std::vector<std::vector<l1t::L1Candidate> > non_iso_row_presorted_energies_matrix_sig_p =
        sort_by_row_in_groups(presorted_non_iso_matrix_sig_p, N_EGAMMA_FIRST_GROUP_SIZE);
    std::vector<std::vector<l1t::L1Candidate> > non_iso_row_presorted_energies_matrix_sig_m =
        sort_by_row_in_groups(presorted_non_iso_matrix_sig_m, N_EGAMMA_FIRST_GROUP_SIZE);

    //3
    std::vector<std::vector<l1t::L1Candidate> > iso_super_sorted_energies_matrix_sig_p =
        super_sort_matrix_rows(iso_row_presorted_energies_matrix_sig_p, N_EGAMMA_FIRST_GROUP_SIZE, N_KEEP_EGAMMA);
    std::vector<std::vector<l1t::L1Candidate> > iso_super_sorted_energies_matrix_sig_m =
        super_sort_matrix_rows(iso_row_presorted_energies_matrix_sig_m, N_EGAMMA_FIRST_GROUP_SIZE, N_KEEP_EGAMMA);
    std::vector<std::vector<l1t::L1Candidate> > non_iso_super_sorted_energies_matrix_sig_p =
        super_sort_matrix_rows(non_iso_row_presorted_energies_matrix_sig_p, N_EGAMMA_FIRST_GROUP_SIZE, N_KEEP_EGAMMA);
    std::vector<std::vector<l1t::L1Candidate> > non_iso_super_sorted_energies_matrix_sig_m =
        super_sort_matrix_rows(non_iso_row_presorted_energies_matrix_sig_m, N_EGAMMA_FIRST_GROUP_SIZE, N_KEEP_EGAMMA);
    //combine plus and minus
    std::vector<std::vector<l1t::L1Candidate> > iso_super_sorted_energies_matrix_sig(
        N_EGAMMA_FIRST_GROUPS, std::vector<l1t::L1Candidate>(N_KEEP_EGAMMA));
    std::vector<std::vector<l1t::L1Candidate> > non_iso_super_sorted_energies_matrix_sig(
        N_EGAMMA_FIRST_GROUPS, std::vector<l1t::L1Candidate>(N_KEEP_EGAMMA));
    for (int r = 0; r < N_EGAMMA_FIRST_GROUPS / 2; r++) {
      iso_super_sorted_energies_matrix_sig[r] = iso_super_sorted_energies_matrix_sig_m[r];
      iso_super_sorted_energies_matrix_sig[r + N_EGAMMA_FIRST_GROUPS / 2] = iso_super_sorted_energies_matrix_sig_p[r];
      non_iso_super_sorted_energies_matrix_sig[r] = non_iso_super_sorted_energies_matrix_sig_m[r];
      non_iso_super_sorted_energies_matrix_sig[r + N_EGAMMA_FIRST_GROUPS / 2] =
          non_iso_super_sorted_energies_matrix_sig_p[r];
    }

    //4
    std::vector<std::vector<l1t::L1Candidate> > iso_stage2_row_sorted_matrix_sig =
        sort_by_row_in_groups(iso_super_sorted_energies_matrix_sig, N_EGAMMA_SECOND_GROUP_SIZE);
    std::vector<std::vector<l1t::L1Candidate> > non_iso_stage2_row_sorted_matrix_sig =
        sort_by_row_in_groups(non_iso_super_sorted_energies_matrix_sig, N_EGAMMA_SECOND_GROUP_SIZE);

    //5
    std::vector<std::vector<l1t::L1Candidate> > iso_stage2_super_sorted_matrix_sig =
        super_sort_matrix_rows(iso_stage2_row_sorted_matrix_sig, N_EGAMMA_SECOND_GROUP_SIZE, N_KEEP_EGAMMA);
    std::vector<std::vector<l1t::L1Candidate> > non_iso_stage2_super_sorted_matrix_sig =
        super_sort_matrix_rows(non_iso_stage2_row_sorted_matrix_sig, N_EGAMMA_SECOND_GROUP_SIZE, N_KEEP_EGAMMA);

    //Prepare output
    std::vector<l1t::L1Candidate> sorted_iso_egammas = iso_stage2_super_sorted_matrix_sig[0];
    std::vector<l1t::L1Candidate> sorted_noniso_egammas = non_iso_stage2_super_sorted_matrix_sig[0];

    //verbose = false;

    for (unsigned int i = 0; i < 4; ++i) {
      auto const& tmp = sorted_iso_egammas[i];
      output->emplace_back(tmp.p4(), tmp.hwPt(), tmp.hwEta(), tmp.hwPhi(), tmp.hwQual(), 1 /*Iso*/);
    }
    for (unsigned int i = 0; i < 4; ++i) {
      auto const& tmp = sorted_noniso_egammas[i];
      output->emplace_back(tmp.p4(), tmp.hwPt(), tmp.hwEta(), tmp.hwPhi(), tmp.hwQual(), tmp.hwIso());
    }
  }

  void SortTaus(std::vector<l1t::Tau>* input, std::vector<l1t::Tau>* output) {
    const int CENTRAL_ETA_SLICES = 14;
    const int N_PHI_GROUPS = 5;
    const int N_PRESORTED_ROWS_CENTRAL = CENTRAL_ETA_SLICES * N_PHI_GROUPS;
    const int PRESORT_DEPTH = 4;
    const int N_KEEP_CENTRAL = 4;
    const int N_ETA_GROUP_SIZE_CENTRAL = 4;
    const int N_ETA_GROUPS_CENTRAL = 4;

    const int cen_nrows = 18;
    const int cen_ncols = 14;

    std::vector<std::vector<l1t::L1Candidate> > cen_input_energy(cen_nrows, std::vector<l1t::L1Candidate>(cen_ncols));

    for (std::vector<l1t::Tau>::const_iterator injet = input->begin(); injet != input->end(); ++injet) {
      if (injet->hwEta() >= 4 && injet->hwEta() <= 17) {
        unsigned int myrow = gt_to_fw_phi_map[injet->hwPhi()];
        unsigned int mycol = injet->hwEta() - 4;  //hardcoding is bad
        cen_input_energy[myrow][mycol] = *injet;
      } else
        edm::LogError("HardwareTauSort") << "Region out of bounds: " << injet->hwEta();
    }

    for (int i = 0; i < cen_nrows; ++i)
      for (int j = 0; j < cen_ncols; ++j) {
        if (cen_input_energy[i][j].hwPt() == 0) {
          cen_input_energy[i][j].setHwPhi(fw_to_gt_phi_map[i]);
          cen_input_energy[i][j].setHwEta(4 + j);
        }
      }

    //Each CLK is one clock

    //CLK 1
    std::vector<std::vector<l1t::L1Candidate> > presorted_energies_matrix_sig =
        presort(cen_input_energy, N_PRESORTED_ROWS_CENTRAL, PRESORT_DEPTH);
    //CLK 2
    std::vector<std::vector<l1t::L1Candidate> > row_presorted_energies_matrix_sig =
        sort_by_row_in_groups(presorted_energies_matrix_sig, N_PHI_GROUPS);
    //CLK 3
    std::vector<std::vector<l1t::L1Candidate> > sorted_eta_slices_energies_matrix_sig =
        super_sort_matrix_rows(row_presorted_energies_matrix_sig, N_PHI_GROUPS, N_KEEP_CENTRAL);
    //CLK 4
    std::vector<std::vector<l1t::L1Candidate> > row_presorted_eta_slices_energies_matrix_sig =
        sort_by_row_in_groups(sorted_eta_slices_energies_matrix_sig, N_ETA_GROUP_SIZE_CENTRAL);
    //CLK 5
    std::vector<std::vector<l1t::L1Candidate> > sorted_eta_groups_energies_matrix_sig =
        super_sort_matrix_rows(row_presorted_eta_slices_energies_matrix_sig, N_ETA_GROUP_SIZE_CENTRAL, N_KEEP_CENTRAL);
    //CLK 6
    std::vector<std::vector<l1t::L1Candidate> > row_presorted_eta_groups_energies_matrix_sig =
        sort_by_row_in_groups(sorted_eta_groups_energies_matrix_sig, N_ETA_GROUPS_CENTRAL);
    //CLK 7
    std::vector<std::vector<l1t::L1Candidate> > sorted_final_energies_matrix_sig =
        super_sort_matrix_rows(row_presorted_eta_groups_energies_matrix_sig, N_ETA_GROUPS_CENTRAL, N_KEEP_CENTRAL);

    for (unsigned int i = 0; i < 4; ++i) {
      auto const& tmp = sorted_final_energies_matrix_sig[0][i];
      output->emplace_back(tmp.p4(), tmp.hwPt(), tmp.hwEta(), tmp.hwPhi(), tmp.hwQual(), tmp.hwIso());
    }
  }
}  // namespace l1t
