#include "feature_manager.h"

int FeaturePerId::endFrame() { return start_frame + feature_per_frame.size() - 1; }

FeatureManager::FeatureManager(Matrix3d _Rs[]) : Rs(_Rs) {
  for (int i = 0; i < NUM_OF_CAM; i++) ric[i].setIdentity();
}

void FeatureManager::setRic(Matrix3d _ric[]) {
  for (int i = 0; i < NUM_OF_CAM; i++) ric[i] = _ric[i];
}

void FeatureManager::clearState() { feature.clear(); }

int FeatureManager::getFeatureCount() {
  int cnt = 0;
  for (auto &it : feature) {
    it.used_num = it.feature_per_frame.size();
    if (it.used_num >= 2 && it.start_frame < WINDOW_SIZE - 2) cnt++;
  }
  return cnt;
}

bool FeatureManager::addFeatureCheckParallax(int frame_count,
                                             const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image,
                                             double td) {
  ROS_DEBUG("input feature: %d", (int)image.size());
  ROS_DEBUG("num of feature: %d", getFeatureCount());

  double parallax_sum = 0;
  int parallax_num = 0;
  last_track_num = 0;
  for (auto &id_pts : image) {
    FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

    int feature_id = id_pts.first;

    // 在feature列表中寻找id为feature_id的feature
    auto it = find_if(feature.begin(), feature.end(),
                      [feature_id](const FeaturePerId &it) { return it.feature_id == feature_id; });

    // 如果该Feature不在Features列表中，则将<FeatureID,Start_frame>存入到Feature列表中
    if (it == feature.end()) {
      feature.push_back(FeaturePerId(feature_id, frame_count));
      feature.back().feature_per_frame.push_back(f_per_fra);
    } else if (it->feature_id == feature_id) {  // 如果该Feature之前被观测到过
      it->feature_per_frame.push_back(f_per_fra);
      last_track_num++;
    }
  }

  // 如果滑窗内的关键帧的个数小于2或者总共被跟踪到的次数小于20
  if (frame_count < 2 || last_track_num < 20) return true;

  // 计算共视关系， parallax_num为满足要求的Feature的个数
  for (auto &it_per_id : feature) {
    // 至少有两帧观测到该特征点(起始帧要小于倒数第二帧，终止帧要大于倒数第二帧，这样至少有三帧观测到该Feature(包括当前帧))
    // 然后比较观测到该faeature的倒数第二帧和倒数第三帧的视差
    if (it_per_id.start_frame <= frame_count - 2 &&
        it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1) {
      parallax_sum += compensatedParallax2(it_per_id, frame_count);
      parallax_num++;
    }
  }

  if (parallax_num == 0) {
    return true;
  } else  // 平均视差视差要大于某个阈值, MIN_PARALLAX=10，大约是10个像素点
  {
    ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
    ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
    return parallax_sum / parallax_num >= MIN_PARALLAX;
  }
}

/**
 * @brief 选取两帧之间共有的Features
 * @param frame_count_l
 * @param frame_count_r
 * @return
 */
vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r) {
  vector<pair<Vector3d, Vector3d>> corres;
  for (auto &it : feature) {
    // 保证两帧的id大于当前特征点的起始id小于当前特征点的终止id
    if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r) {
      int idx_l = frame_count_l - it.start_frame;
      int idx_r = frame_count_r - it.start_frame;

      Vector3d a = it.feature_per_frame[idx_l].point;
      Vector3d b = it.feature_per_frame[idx_r].point;

      corres.push_back(make_pair(a, b));
    }
  }
  return corres;
}

void FeatureManager::setDepth(const VectorXd &x) {
  int feature_index = -1;
  for (auto &it_per_id : feature) {
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;

    it_per_id.estimated_depth = 1.0 / x(++feature_index);
    // ROS_INFO("feature id %d , start_frame %d, depth %f ", it_per_id->feature_id, it_per_id-> start_frame,
    // it_per_id->estimated_depth);
    if (it_per_id.estimated_depth < 0) {
      it_per_id.solve_flag = 2;
    } else
      it_per_id.solve_flag = 1;
  }
}

void FeatureManager::removeFailures() {
  for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) {
    it_next++;
    if (it->solve_flag == 2) feature.erase(it);
  }
}

void FeatureManager::clearDepth(const VectorXd &x) {
  int feature_index = -1;
  for (auto &it_per_id : feature) {
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;
    it_per_id.estimated_depth = 1.0 / x(++feature_index);
  }
}

VectorXd FeatureManager::getDepthVector() {
  VectorXd dep_vec(getFeatureCount());
  int feature_index = -1;
  for (auto &it_per_id : feature) {
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;
#if 1
    dep_vec(++feature_index) = 1. / it_per_id.estimated_depth;
#else
    dep_vec(++feature_index) = it_per_id->estimated_depth;
#endif
  }
  return dep_vec;
}

void FeatureManager::triangulate(Vector3d Ps[], Vector3d tic[], Matrix3d ric[]) {
  for (auto &it_per_id : feature) {
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;

    if (it_per_id.estimated_depth > 0) continue;

    int imu_i = it_per_id.start_frame;
    int imu_j = imu_i - 1;

    ROS_ASSERT(NUM_OF_CAM == 1);

    Eigen::MatrixXd svd_A(2 * it_per_id.feature_per_frame.size(), 4);
    int svd_idx = 0;

    Eigen::Vector3d t0 = Ps[imu_i] + Rs[imu_i] * tic[0];
    Eigen::Matrix3d R0 = Rs[imu_i] * ric[0];

    Eigen::Matrix<double, 3, 4> P0;
    P0.leftCols<3>() = Eigen::Matrix3d::Identity();
    P0.rightCols<1>() = Eigen::Vector3d::Zero();

    for (auto &it_per_frame : it_per_id.feature_per_frame) {
      imu_j++;

      Eigen::Vector3d t1 = Ps[imu_j] + Rs[imu_j] * tic[0];
      Eigen::Matrix3d R1 = Rs[imu_j] * ric[0];

      Eigen::Vector3d t = R0.transpose() * (t1 - t0);
      Eigen::Matrix3d R = R0.transpose() * R1;

      Eigen::Matrix<double, 3, 4> P;
      P.leftCols<3>() = R.transpose();
      P.rightCols<1>() = -R.transpose() * t;

      Eigen::Vector3d f = it_per_frame.point.normalized();

      svd_A.row(svd_idx++) = f[0] * P.row(2) - f[2] * P.row(0);
      svd_A.row(svd_idx++) = f[1] * P.row(2) - f[2] * P.row(1);

      if (imu_i == imu_j) continue;
    }
    ROS_ASSERT(svd_idx == svd_A.rows());

    Eigen::Vector4d svd_V = Eigen::JacobiSVD<Eigen::MatrixXd>(svd_A, Eigen::ComputeThinV).matrixV().rightCols<1>();

    it_per_id.estimated_depth = svd_V[2] / svd_V[3];

    if (it_per_id.estimated_depth < 0.1) {
      it_per_id.estimated_depth = INIT_DEPTH;
    }
  }
}

void FeatureManager::removeOutlier() {
  ROS_BREAK();
  int i = -1;
  for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) {
    it_next++;
    i += it->used_num != 0;
    if (it->used_num != 0 && it->is_outlier == true) {
      feature.erase(it);
    }
  }
}

void FeatureManager::removeBackShiftDepth(Eigen::Matrix3d marg_R,
                                          Eigen::Vector3d marg_P,
                                          Eigen::Matrix3d new_R,
                                          Eigen::Vector3d new_P) {
  for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) {
    it_next++;

    if (it->start_frame != 0)
      it->start_frame--;
    else {  // 对于原来的第一帧，就要丢弃了，在丢弃之前，把相关信息传给第2帧
      Eigen::Vector3d uv_i = it->feature_per_frame[0].point;
      it->feature_per_frame.erase(it->feature_per_frame.begin());

      if (it->feature_per_frame.size() < 2) {  // 如果丢弃的帧，没啥特征点，直接丢弃
        feature.erase(it);
        continue;
      } else {
        Eigen::Vector3d pts_i = uv_i * it->estimated_depth;
        Eigen::Vector3d w_pts_i = marg_R * pts_i + marg_P;
        Eigen::Vector3d pts_j = new_R.transpose() * (w_pts_i - new_P);
        double dep_j = pts_j(2);
        if (dep_j > 0)
          it->estimated_depth = dep_j;
        else
          it->estimated_depth = INIT_DEPTH;
      }
    }
    // remove tracking-lost feature after marginalize
    /*
    if (it->endFrame() < WINDOW_SIZE - 1)
    {
        feature.erase(it);
    }
    */
  }
}

void FeatureManager::removeBack() {
  for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) {
    it_next++;

    if (it->start_frame != 0)
      it->start_frame--;
    else {
      it->feature_per_frame.erase(it->feature_per_frame.begin());
      if (it->feature_per_frame.size() == 0) feature.erase(it);
    }
  }
}

void FeatureManager::removeFront(int frame_count) {
  for (auto it = feature.begin(), it_next = feature.begin(); it != feature.end(); it = it_next) {
    it_next++;

    if (it->start_frame == frame_count) {
      it->start_frame--;
    } else {
      int j = WINDOW_SIZE - 1 - it->start_frame;
      if (it->endFrame() < frame_count - 1) continue;
      it->feature_per_frame.erase(it->feature_per_frame.begin() + j);
      if (it->feature_per_frame.size() == 0) feature.erase(it);
    }
  }
}

/**
 * @brief 实际是求取该特征点在两帧的归一化平面上的坐标点的距离
 * @param it_per_id
 * @param frame_count
 * @return
 */
double FeatureManager::compensatedParallax2(const FeaturePerId &it_per_id, int frame_count) {
  // check the second last frame is keyframe or not
  // parallax betwwen seconde last frame and third last frame
  const FeaturePerFrame &frame_i = it_per_id.feature_per_frame[frame_count - 2 - it_per_id.start_frame];
  const FeaturePerFrame &frame_j = it_per_id.feature_per_frame[frame_count - 1 - it_per_id.start_frame];

  Vector3d p_j = frame_j.point;

  double u_j = p_j(0);
  double v_j = p_j(1);

  Vector3d p_i = frame_i.point;
  Vector3d p_i_comp;

  // int r_i = frame_count - 2;
  // int r_j = frame_count - 1;
  // p_i_comp = ric[camera_id_j].transpose() * Rs[r_j].transpose() * Rs[r_i] * ric[camera_id_i] * p_i;
  p_i_comp = p_i;

  double dep_i = p_i(2);
  double u_i = p_i(0) / dep_i;
  double v_i = p_i(1) / dep_i;
  double du = u_i - u_j;
  double dv = v_i - v_j;

  double dep_i_comp = p_i_comp(2);
  double u_i_comp = p_i_comp(0) / dep_i_comp;
  double v_i_comp = p_i_comp(1) / dep_i_comp;
  double du_comp = u_i_comp - u_j;
  double dv_comp = v_i_comp - v_j;

  double ans = 0;
  ans = max(ans, sqrt(min(du * du + dv * dv, du_comp * du_comp + dv_comp * dv_comp)));

  return ans;
}