use super::param::HoughLine;
use crate::utils::{CartesianEquation, EqAffine, Pt};
use opencv::core::{add, subtract, Mat, VecN, Vector, CV_8UC3};
//use opencv::prelude::MatTraitConst;
use opencv::imgproc::{cvt_color, hough_lines_p, COLOR_GRAY2BGR};
use opencv::prelude::*; //MatTraitConst;
use opencv::types::VectorOfVec4i;
use opencv::Result;

pub fn mix_borders(background: &Mat, borders: Vec<Mat>) -> Result<Mat> {
    let (row, col) = (background.rows(), background.cols());
    //let mask = Mat::default();
    let v: VecN<f64, 4> = VecN::new(0., 0., 0., 0.);
    let mut sum_diff_0 = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
    let mut sum_diff_1 = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
    let mask = Mat::default();
    let mut tmp = Mat::default();

    // on va faire la somme des difference
    // on va les ajouter a l'image du fond
    for bord in borders {
        //let diff = image_diff(&bord, background)?;
        //add(&diff, &mid, &mut diff_bgr_2, &mask, -1)?;
        let mut diff = Mat::default();
        subtract(&bord, background, &mut diff, &mask, -1)?;
        add(&diff, &sum_diff_0, &mut tmp, &mask, -1)?;
        sum_diff_0 = tmp.clone();

        subtract(background, &bord, &mut diff, &mask, -1)?;
        add(&diff, &sum_diff_1, &mut tmp, &mask, -1)?;
        sum_diff_1 = tmp.clone();
    }
    //let v: VecN<f64, 4> = VecN::new(128., 128., 128., 128.);
    //let mid = Mat::new_rows_cols_with_default(row, col, CV_8UC3, v)?;
    let mut tmp = Mat::default();
    //let mut tmp2 = Mat::default();
    let mut mix = Mat::default();
    add(&sum_diff_0, &background, &mut tmp, &mask, -1)?;
    subtract(&tmp, &sum_diff_1, &mut mix, &mask, -1)?;

    Ok(mix)
}

//impl Add for (f64, f64) {
//}
pub fn bord_mult(pt: Vec<(f64, f64)>, factor: f64) -> Vec<(f64, f64)> {
    let pt: Vec<Pt> = pt.iter().map(|p| Pt::from(p)).collect();

    let mut pa = vec![];
    let mut pb = vec![];
    for i in 0..pt.len() {
        let k = (i + pt.len() - 1) % pt.len();
        let j = (i + 1) % pt.len();
        pa.push((pt[i] - pt[j]) * factor + pt[j]);
        pb.push((pt[i] - pt[k]) * factor + pt[k]);
    }

    let mut eq = vec![];
    for i in 0..pt.len() {
        let j = (i + 1) % pt.len();
        eq.push(CartesianEquation::new_from_pt(&pb[i], &pa[j]));
    }

    let mut p_out = vec![];
    for i in 0..pt.len() {
        let k = (i + pt.len() - 1) % pt.len();
        p_out.push(eq[i].intersection(&eq[k]).unwrap()); // TODO: faire un truc pour le unwrap...
                                                         // normalement c'est un gars sur ta
                                                         // compris ;)... mais bon... un alignement
                                                         // malencontreux ca arrive vite
    }

    p_out.iter().map(|p| (p.x, p.y)).collect()
}

// en fait ca marche pas dutout...next time
pub fn bord_mult_v2(pt: Vec<(f64, f64)>, factor: f64) -> Vec<(f64, f64)> {
    let mut pt: Vec<Pt> = pt.iter().map(|p| Pt::from(p)).collect();

    let mut pn = vec![];
    for i in 0..pt.len() {
        let j = (i + 2) % pt.len();
        pn.push((pt[i] - pt[j]) * factor + pt[j]);
    }

    pn.iter().map(|p| (p.x, p.y)).collect()
}

pub fn get_intersection(pts: &[((f64, f64), (f64, f64))]) -> Vec<(f64, f64)> {
    let mut eq_cart = vec![];
    for i in 0..pts.len() {
        eq_cart.push(CartesianEquation::new_from_tuple(pts[i]));
    }

    let mut points = vec![];
    for i in 0..eq_cart.len() {
        let id_next = (i + 1) % eq_cart.len();
        let pt = eq_cart[i].intersection(&eq_cart[id_next]).unwrap(); // TODO verifier quand meme la sortie au lieu de unwrap salement... xD
        points.push((pt.x, pt.y));
    }

    points
}

pub fn get_extermities(lines: &Vector<VecN<i32, 4>>, id: usize) -> ((f64, f64), (f64, f64)) {
    let mut p0: (f64, f64) = (0., 0.);
    let mut p1: (f64, f64) = (0., 0.);
    let (mut min, mut max): (f64, f64) = (f64::MAX, f64::MIN);
    //let mut eq: (f64, f64, f64) = (0., 0., 0.);
    let mut dst_sum = 0.;
    let mut v_eq = vec![];

    // on cherche les extremite
    for l in lines {
        // rename value and switch x and y if necessery
        let (mut a0, mut b0, mut a1, mut b1) = if id % 2 == 0 {
            (l[0] as f64, l[1] as f64, l[2] as f64, l[3] as f64)
        } else {
            (l[1] as f64, l[0] as f64, l[3] as f64, l[2] as f64) // switch x <-> y
        };

        // reorder if not
        if a0 > a1 {
            (a0, b0, a1, b1) = (a1, b1, a0, b0);
        }

        // update min/max
        min = min.min(a0).min(a1);
        max = max.max(a0).max(a1);

        // cancel computation if devide by zero
        if a1 - a0 == 0. {
            continue;
        }

        let eq = EqAffine::new(a0, b0, a1, b1);
        dst_sum += eq.dst;
        v_eq.push(eq);
    }

    p0.0 = min;
    p1.0 = max;
    for eq in v_eq {
        p0.1 += eq.get_val_dst(min);
        p1.1 += eq.get_val_dst(max);
    }
    p0.1 /= dst_sum;
    p1.1 /= dst_sum;

    // revert x-y if already reverted previously
    if id % 2 != 0 {
        p0 = (p0.1, p0.0);
        p1 = (p1.1, p1.0);
    }

    (p0, p1)
}

pub fn probabilistic_hough(
    edges: &Mat,
    hough_param: &HoughLine,
    id: usize,
) -> Result<Vector<VecN<i32, 4>>> {
    let mut p_lines = VectorOfVec4i::new();
    let mut probabalistic_hough = Mat::default();

    cvt_color(edges, &mut probabalistic_hough, COLOR_GRAY2BGR, 0)?;

    // 2. Use Probabilistic Hough Transform
    let p = hough_param.get_param();
    hough_lines_p(
        edges,
        &mut p_lines,
        p.rho,
        p.theta,
        p.treshold,
        p.min_length,
        p.max_line_gap,
    )?;

    Ok(p_lines)
}