/**
 * Run the optical flow with EDGEFLOW on a new image frame
 * @param[in] *opticflow The opticalflow structure that keeps track of previous images
 * @param[in] *state The state of the drone
 * @param[in] *img The image frame to calculate the optical flow from
 * @param[out] *result The optical flow result
 */
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                       struct opticflow_result_t *result)
{
  // Define Static Variables
  static struct edge_hist_t edge_hist[MAX_HORIZON];
  static uint8_t current_frame_nr = 0;
  struct edge_flow_t edgeflow;
  static uint8_t previous_frame_offset[2] = {1, 1};

  // Define Normal variables
  struct edgeflow_displacement_t displacement;
  displacement.x = malloc(sizeof(int32_t) * img->w);
  displacement.y = malloc(sizeof(int32_t) * img->h);

  // If the methods just switched to this one, reintialize the
  // array of edge_hist structure.
  if (opticflow->just_switched_method == 1) {
    int i;
    for (i = 0; i < MAX_HORIZON; i++) {
      edge_hist[i].x = malloc(sizeof(int32_t) * img->w);
      edge_hist[i].y = malloc(sizeof(int32_t) * img->h);
      FLOAT_RATES_ZERO(edge_hist[i].rates);
    }
  }

  uint16_t disp_range;
  if (opticflow->search_distance < DISP_RANGE_MAX) {
    disp_range = opticflow->search_distance;
  } else {
    disp_range = DISP_RANGE_MAX;
  }

  uint16_t window_size;

  if (opticflow->window_size < MAX_WINDOW_SIZE) {
    window_size = opticflow->window_size;
  } else {
    window_size = MAX_WINDOW_SIZE;
  }

  uint16_t RES = opticflow->subpixel_factor;

  //......................Calculating EdgeFlow..................... //

  // Calculate current frame's edge histogram
  int32_t *edge_hist_x = edge_hist[current_frame_nr].x;
  int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
  calculate_edge_histogram(img, edge_hist_x, 'x', 0);
  calculate_edge_histogram(img, edge_hist_y, 'y', 0);


  // Copy frame time and angles of image to calculated edge histogram
  edge_hist[current_frame_nr].frame_time = img->ts;
  edge_hist[current_frame_nr].rates = state->rates;

  // Calculate which previous edge_hist to compare with the current
  uint8_t previous_frame_nr[2];
  calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);

  //Select edge histogram from the previous frame nr
  int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
  int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;

  //Calculate the corresponding derotation of the two frames
  int16_t der_shift_x = 0;
  int16_t der_shift_y = 0;

  if (opticflow->derotation) {
    der_shift_x = (int16_t)(edge_hist[current_frame_nr].rates.p  /
                            result->fps *
                            (float)img->w / (OPTICFLOW_FOV_W) * opticflow->derotation_correction_factor_x);
    der_shift_y = (int16_t)(edge_hist[current_frame_nr].rates.q /
                            result->fps *
                            (float)img->h / (OPTICFLOW_FOV_H) * opticflow->derotation_correction_factor_y);
  }

  // Estimate pixel wise displacement of the edge histograms for x and y direction
  calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
                              displacement.x, img->w,
                              window_size, disp_range,  der_shift_x);
  calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
                              displacement.y, img->h,
                              window_size, disp_range, der_shift_y);

  // Fit a line on the pixel displacement to estimate
  // the global pixel flow and divergence (RES is resolution)
  line_fit(displacement.x, &edgeflow.div_x,
           &edgeflow.flow_x, img->w,
           window_size + disp_range, RES);
  line_fit(displacement.y, &edgeflow.div_y,
           &edgeflow.flow_y, img->h,
           window_size + disp_range, RES);

  /* Save Resulting flow in results
   * Warning: The flow detected here is different in sign
   * and size, therefore this will be multiplied with
   * the same subpixel factor and -1 to make it on par with
   * the LK algorithm of t opticalflow_calculator.c
   * */
  edgeflow.flow_x = -1 * edgeflow.flow_x;
  edgeflow.flow_y = -1 * edgeflow.flow_y;

  result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
  result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];

  //Fill up the results optic flow to be on par with LK_fast9
  result->flow_der_x =  result->flow_x;
  result->flow_der_y =  result->flow_y;
  result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
  result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
  result->divergence = (float)edgeflow.flow_x / RES;
  result->div_size = 0.0f;
  result->noise_measurement = 0.0f;
  result->surface_roughness = 0.0f;

  //......................Calculating VELOCITY ..................... //

  /*Estimate fps per direction
   * This is the fps with adaptive horizon for subpixel flow, which is not similar
   * to the loop speed of the algorithm. The faster the quadcopter flies
   * the higher it becomes
  */
  float fps_x = 0;
  float fps_y = 0;
  float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.;
  float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.;
  fps_x = 1 / (time_diff_x);
  fps_y = 1 / (time_diff_y);

  result->fps = fps_x;

  // Calculate velocity
  float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
  float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);

  //Apply a  median filter to the velocity if wanted
  if (opticflow->median_filter == true) {
    result->vel_x = (float)update_median_filter(&vel_x_filt, (int32_t)(vel_x * 1000)) / 1000;
    result->vel_y = (float)update_median_filter(&vel_y_filt, (int32_t)(vel_y * 1000)) / 1000;
  } else {
    result->vel_x = vel_x;
    result->vel_y = vel_y;
  }

  result->noise_measurement = 0.2;



#if OPTICFLOW_SHOW_FLOW
  draw_edgeflow_img(img, edgeflow, prev_edge_histogram_x, edge_hist_x);
#endif
  // Increment and wrap current time frame
  current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
}
/**
 * Run the optical flow with EDGEFLOW on a new image frame
 * @param[in] *opticflow The opticalflow structure that keeps track of previous images
 * @param[in] *state The state of the drone
 * @param[in] *img The image frame to calculate the optical flow from
 * @param[out] *result The optical flow result
 */
void calc_edgeflow_tot(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img,
                       struct opticflow_result_t *result)
{
    // Define Static Variables
    static struct edge_hist_t edge_hist[MAX_HORIZON];
    static uint8_t current_frame_nr = 0;
    static struct edge_flow_t edgeflow;
    static uint8_t previous_frame_offset[2] = {1, 1};

    // Define Normal variables
    struct edgeflow_displacement_t displacement;
    uint16_t disp_range;
    if (opticflow->search_distance < DISP_RANGE_MAX) {
        disp_range = opticflow->search_distance;
    } else {
        disp_range = DISP_RANGE_MAX;
    }

    uint16_t window_size;

    if (opticflow->window_size < MAX_WINDOW_SIZE) {
        window_size = opticflow->window_size;
    } else {
        window_size = MAX_WINDOW_SIZE;
    }

    uint16_t RES = opticflow->subpixel_factor;

    //......................Calculating EdgeFlow..................... //

    // Calculate current frame's edge histogram
    int32_t *edge_hist_x = edge_hist[current_frame_nr].x;
    int32_t *edge_hist_y = edge_hist[current_frame_nr].y;
    calculate_edge_histogram(img, edge_hist_x, 'x', 0);
    calculate_edge_histogram(img, edge_hist_y, 'y', 0);

    // Copy frame time and angles of image to calculated edge histogram
    memcpy(&edge_hist[current_frame_nr].frame_time, &img->ts, sizeof(struct timeval));
    edge_hist[current_frame_nr].pitch = state->theta;
    edge_hist[current_frame_nr].roll = state->phi;

    // Calculate which previous edge_hist to compare with the current
    uint8_t previous_frame_nr[2];
    calc_previous_frame_nr(result, opticflow, current_frame_nr, previous_frame_offset, previous_frame_nr);

    //Select edge histogram from the previous frame nr
    int32_t *prev_edge_histogram_x = edge_hist[previous_frame_nr[0]].x;
    int32_t *prev_edge_histogram_y = edge_hist[previous_frame_nr[1]].y;

    //Calculate the corresponding derotation of the two frames
    int16_t der_shift_x = 0;
    int16_t der_shift_y = 0;

    if (opticflow->derotation) {
        der_shift_x = -(int16_t)((edge_hist[previous_frame_nr[0]].roll - edge_hist[current_frame_nr].roll) *
                                 (float)img->w / (OPTICFLOW_FOV_W));
        der_shift_y = -(int16_t)((edge_hist[previous_frame_nr[1]].pitch - edge_hist[current_frame_nr].pitch) *
                                 (float)img->h / (OPTICFLOW_FOV_H));
    }

    // Estimate pixel wise displacement of the edge histograms for x and y direction
    calculate_edge_displacement(edge_hist_x, prev_edge_histogram_x,
                                displacement.x, img->w,
                                window_size, disp_range,  der_shift_x);
    calculate_edge_displacement(edge_hist_y, prev_edge_histogram_y,
                                displacement.y, img->h,
                                window_size, disp_range, der_shift_y);

    // Fit a line on the pixel displacement to estimate
    // the global pixel flow and divergence (RES is resolution)
    line_fit(displacement.x, &edgeflow.div_x,
             &edgeflow.flow_x, img->w,
             window_size + disp_range, RES);
    line_fit(displacement.y, &edgeflow.div_y,
             &edgeflow.flow_y, img->h,
             window_size + disp_range, RES);

    /* Save Resulting flow in results
     * Warning: The flow detected here is different in sign
     * and size, therefore this will be multiplied with
     * the same subpixel factor and -1 to make it on par with
     * the LK algorithm of t opticalflow_calculator.c
     * */
    edgeflow.flow_x = -1 * edgeflow.flow_x;
    edgeflow.flow_y = -1 * edgeflow.flow_y;

    result->flow_x = (int16_t)edgeflow.flow_x / previous_frame_offset[0];
    result->flow_y = (int16_t)edgeflow.flow_y / previous_frame_offset[1];

    //Fill up the results optic flow to be on par with LK_fast9
    result->flow_der_x =  result->flow_x;
    result->flow_der_y =  result->flow_y;
    result->corner_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
    result->tracked_cnt = getAmountPeaks(edge_hist_x, 500 , img->w);
    result->divergence = (float)edgeflow.flow_x / RES;
    result->div_size = 0.0f;
    result->noise_measurement = 0.0f;
    result->surface_roughness = 0.0f;

    //......................Calculating VELOCITY ..................... //

    /*Estimate fps per direction
     * This is the fps with adaptive horizon for subpixel flow, which is not similar
     * to the loop speed of the algorithm. The faster the quadcopter flies
     * the higher it becomes
    */
    float fps_x = 0;
    float fps_y = 0;
    float time_diff_x = (float)(timeval_diff(&edge_hist[previous_frame_nr[0]].frame_time, &img->ts)) / 1000.;
    float time_diff_y = (float)(timeval_diff(&edge_hist[previous_frame_nr[1]].frame_time, &img->ts)) / 1000.;
    fps_x = 1 / (time_diff_x);
    fps_y = 1 / (time_diff_y);

    result->fps = fps_x;

    // Calculate velocity
    float vel_x = edgeflow.flow_x * fps_x * state->agl * OPTICFLOW_FOV_W / (img->w * RES);
    float vel_y = edgeflow.flow_y * fps_y * state->agl * OPTICFLOW_FOV_H / (img->h * RES);
    result->vel_x = vel_x;
    result->vel_y = vel_y;

    /* Rotate velocities from camera frame coordinates to body coordinates.
    * IMPORTANT This frame to body orientation should be the case for the Parrot
    * ARdrone and Bebop, however this can be different for other quadcopters
    * ALWAYS double check!
    */
    result->vel_body_x = - vel_y;
    result->vel_body_y = vel_x;

#if OPTICFLOW_DEBUG && OPTICFLOW_SHOW_FLOW
    draw_edgeflow_img(img, edgeflow, displacement, *edge_hist_x)
#endif
    // Increment and wrap current time frame
    current_frame_nr = (current_frame_nr + 1) % MAX_HORIZON;
}