use num_complex::Complex;
use std::f64;

pub fn zpk2sos(
    z: &Vec<Complex<f64>>,
    p: &Vec<Complex<f64>>,
    k: f64,
    analog: bool,
    pairing: Option<&str>,
) -> Result<Vec<[f64; 6]>, String> {
    let mut z = z.to_vec();
    let mut p = p.to_vec();
    let k = k;

    let pairing = match pairing {
        Some("nearest") | None if !analog => "nearest",
        Some("minimal") | None if analog => "minimal",
        Some("keep_odd") => "keep_odd",
        Some(s) => panic!("Invalid pairing: {}", s),
        None => unreachable!(),
    };

    if analog && pairing != "minimal" {
        panic!("Analog conversion requires pairing='minimal'");
    }

    if z.is_empty() && p.is_empty() {
        return Ok(vec![[k, 0.0, 0.0, 1.0, 0.0, 0.0]]);
    }

    if pairing != "minimal" {
        let max_len = z.len().max(p.len());
        while z.len() < max_len {
            z.push(Complex::new(0.0, 0.0));
        }
        while p.len() < max_len {
            p.push(Complex::new(0.0, 0.0));
        }

        if p.len() % 2 == 1 && pairing == "nearest" {
            p.push(Complex::new(0.0, 0.0));
            z.push(Complex::new(0.0, 0.0));
        }
    } else if p.len() < z.len() {
        panic!("For analog conversion, number of poles must be >= number of zeros");
    }

    let n_sections = (p.len().max(z.len()) + 1) / 2;

    // Ensure conjugate pairs
    let (mut zc, zr) = cplxreal_complex(&z, None)?;
    zc.extend(zr.iter().map(|&r| Complex::new(r, 0.0)));

    let (mut pc, pr) = cplxreal_complex(&p, None)?;
    pc.extend(pr.iter().map(|&r| Complex::new(r, 0.0)));

    if k.is_nan() {
        panic!("Gain k must be real");
    }

    let mut sos = vec![[0.0; 6]; n_sections];

    let idx_worst = if analog {
        idx_worst_analog
    } else {
        idx_worst_digital
    };

    for si in (0..n_sections).rev() {
        let p1_idx = idx_worst(&pc);
        let p1 = pc.remove(p1_idx);

        if p1.im == 0.0 && pc.iter().all(|c| c.im != 0.0) {
            if pairing != "minimal" && !zc.is_empty() {
                let z1_idx = nearest_real_complex_idx(&zc, p1, "real")?;
                let z1 = zc.remove(z1_idx);
                sos[si] = single_zpksos_complex(&vec![z1, Complex::new(0.0, 0.0)], &vec![p1, Complex::new(0.0, 0.0)], 1.0);
            } else if !zc.is_empty() {
                let z1_idx = nearest_real_complex_idx(&zc, p1, "real")?;
                let z1 = zc.remove(z1_idx);
                sos[si] = single_zpksos_complex(&vec![z1], &vec![p1], 1.0);
            } else {
                sos[si] = single_zpksos_complex(&vec![], &vec![p1], 1.0);
            }
        } else if pc.len() + 1 == zc.len()
            && p1.im != 0.0
            && pc.iter().filter(|c| c.im == 0.0).count() == 1
            && zc.iter().filter(|c| c.im == 0.0).count() == 1
        {
            let z1_idx = nearest_real_complex_idx(&zc, p1, "complex")?;
            let z1 = zc.remove(z1_idx);
            sos[si] = single_zpksos_complex(&vec![z1, z1.conj()], &vec![p1, p1.conj()], 1.0);
        } else {
            let p2: Complex<f64>;
            if p1.im == 0.0 {
                // 取出所有实数元素 (index, &Complex)
                let reals: Vec<(usize, &Complex<f64>)> = pc.iter().enumerate()
                    .filter(|(_, c)| c.im == 0.0)
                    .collect();

                // 提取只包含 Complex<f64> 的 Vec 用于传给 idx_worst
                let real_vals: Vec<Complex<f64>> = reals.iter()
                    .map(|(_, c)| **c)  // 解引用两次，得到 Complex<f64> 复制一份
                    .collect();

                // 找最差索引，但这里返回的是 real_vals 的索引
                let idx_in_real_vals = idx_worst(&real_vals);

                // 再映射回 pc 的索引
                let p2_idx = reals[idx_in_real_vals].0;
                p2 = pc[p2_idx];
                pc.remove(p2_idx);
            } else {
                p2 = p1.conj();
            }

            if !zc.is_empty() {
                let z1_idx = nearest_real_complex_idx(&zc, p1, "any")?;
                let z1 = zc.remove(z1_idx);
                if z1.im != 0.0 {
                    sos[si] = single_zpksos_complex(&vec![z1, z1.conj()], &vec![p1, p2], 1.0);
                } else if !zc.is_empty() {
                    let z2_idx = nearest_real_complex_idx(&zc, p1, "real")?;
                    let z2 = zc.remove(z2_idx);
                    sos[si] = single_zpksos_complex(&vec![z1, z2], &vec![p1, p2], 1.0);
                } else {
                    sos[si] = single_zpksos_complex(&vec![z1], &vec![p1, p2], 1.0);
                }
            } else {
                sos[si] = single_zpksos_complex(&vec![], &vec![p1, p2], 1.0);
            }
        }
    }
    // Apply gain to first section
    for i in 0..3 {
        sos[0][i] *= k;
    }
    Ok(sos)
}



/// 数字系统：寻找最“差”的极点（离单位圆最近）
fn idx_worst_digital(p: &[Complex<f64>]) -> usize {
    p.iter()
        .enumerate()
        .map(|(i, val)| (i, (1.0 - val.norm()).abs()))
        .min_by(|a, b| a.1.partial_cmp(&b.1).unwrap())
        .unwrap()
        .0
}

/// 模拟系统：寻找最“差”的极点（离虚轴最近）
fn idx_worst_analog(p: &[Complex<f64>]) -> usize {
    p.iter()
        .enumerate()
        .map(|(i, val)| (i, val.re.abs()))
        .min_by(|a, b| a.1.partial_cmp(&b.1).unwrap())
        .unwrap()
        .0
}

/// 提取复共轭对和实数值
/// z: 输入数组，元素为实数或复数
/// 返回值: (共轭对平均值数组, 实数数组)
fn cplxreal_complex(z: &[Complex<f64>], tol: Option<f64>) -> Result<(Vec<Complex<f64>>, Vec<f64>), String> {
    if z.is_empty() {
        return Ok((Vec::new(), Vec::new()));
    }

    let tol = tol.unwrap_or(100.0 * f64::EPSILON);

    // 拷贝并排序：先按实部、再按虚部绝对值排序
    let mut z_sorted = z.to_vec();
    z_sorted.sort_by(|a, b| {
        a.re.partial_cmp(&b.re).unwrap_or(std::cmp::Ordering::Equal)
            .then_with(|| a.im.abs().partial_cmp(&b.im.abs()).unwrap_or(std::cmp::Ordering::Equal))
    });

    // 识别实数
    let mut zr = Vec::new();
    let mut complex_parts = Vec::new();

    for &val in &z_sorted {
        if val.im.abs() <= tol * val.norm() {
            zr.push(val.re);
        } else {
            complex_parts.push(val);
        }
    }

    if complex_parts.is_empty() {
        return Ok((Vec::new(), zr));
    }

    // 拆分共轭对：正虚部和负虚部
    let mut zp: Vec<Complex<f64>> = complex_parts.iter().cloned().filter(|x| x.im > 0.0).collect();
    let mut zn: Vec<Complex<f64>> = complex_parts.iter().cloned().filter(|x| x.im < 0.0).collect();

    if zp.len() != zn.len() {
        return Err("Array contains complex value with no matching conjugate.".to_string());
    }

    // 对于 real 相近的一段，按 imag 排序
    // 这里我们简单地按 real 分组，并逐段处理
    zp.sort_by(|a, b| a.re.partial_cmp(&b.re).unwrap_or(std::cmp::Ordering::Equal));
    zn.sort_by(|a, b| a.re.partial_cmp(&b.re).unwrap_or(std::cmp::Ordering::Equal));

    // 再次按 imag 排序每组
    // 省略 run 分组（简化版）：直接对所有进行 imag 排序
    zp.sort_by(|a, b| a.im.abs().partial_cmp(&b.im.abs()).unwrap_or(std::cmp::Ordering::Equal));
    zn.sort_by(|a, b| a.im.abs().partial_cmp(&b.im.abs()).unwrap_or(std::cmp::Ordering::Equal));

    // 检查是否共轭匹配
    for (a, b) in zp.iter().zip(zn.iter()) {
        if (*a - b.conj()).norm() > tol * b.norm() {
            return Err("Array contains complex value with no matching conjugate.".to_string());
        }
    }

    // 取平均以消除数值误差
    let zc: Vec<Complex<f64>> = zp.iter()
        .zip(zn.iter())
        .map(|(a, b)| (*a + b.conj()) / Complex::new(2.0, 0.0))
        .collect();

    Ok((zc, zr))
}

/// 从 `fro` 中选出与 `to` 距离最近的元素的索引
/// `which`: "real" | "complex" | "any"
fn nearest_real_complex_idx(
    fro: &[Complex<f64>],
    to: Complex<f64>,
    which: &str,
) -> Result<usize, String> {
    assert!(which == "real" || which == "complex" || which == "any");

    // 计算每个元素与 to 的距离（模长差）
    let mut indexed_distances: Vec<(usize, f64)> = fro.iter()
        .enumerate()
        .map(|(i, val)| (i, (*val - to).norm()))
        .collect();

    // 按距离升序排序
    indexed_distances.sort_by(|a, b| a.1.partial_cmp(&b.1).unwrap());

    match which {
        "any" => Ok(indexed_distances[0].0),
        "real" => {
            for (i, _) in indexed_distances.iter() {
                if fro[*i].im.abs() <= f64::EPSILON {
                    return Ok(*i);
                }
            }
            Err("No real element found".to_string())
        }
        "complex" => {
            for (i, _) in indexed_distances.iter() {
                if fro[*i].im.abs() > f64::EPSILON {
                    return Ok(*i);
                }
            }
            Err("No complex element found".to_string())
        }
        _ => Err("Invalid `which` argument".to_string())
    }
}
/// 构造单个二阶节 [b0, b1, b2, a0, a1, a2]
pub fn single_zpksos_complex(z: &[Complex<f64>], p: &[Complex<f64>], k: f64) -> [f64; 6] {
    let (b_complex, a_complex) = zpk2tf_complex(z, p, k);

    // 将复数系数转为实数，若虚部为非零，则说明存在数值不一致
    let b: Vec<f64> = b_complex
        .iter()
        .map(|c| {
            if c.im.abs() > 1e-10 {
                panic!("b 系数包含非零虚部: {:?}", c);
            }
            c.re
        })
        .collect();

    let a: Vec<f64> = a_complex
        .iter()
        .map(|c| {
            if c.im.abs() > 1e-10 {
                panic!("a 系数包含非零虚部: {:?}", c);
            }
            c.re
        })
        .collect();
    let mut sos = [0.0; 6];

    // 填充 b 系数（对齐右边）
    let b_start = 3 - b.len();
    for (i, &val) in b.iter().enumerate() {
        sos[b_start + i] = val;
    }

    // 填充 a 系数（对齐右边）
    let a_start = 6 - a.len();
    for (i, &val) in a.iter().enumerate() {
        sos[a_start + i] = val;
    }

    sos
}
/// zpk2tf: 从零点、极点、增益构建滤波器系数，z 和 p 都为复数输入
fn zpk2tf_complex(z: &[Complex<f64>], p: &[Complex<f64>], k: f64) -> (Vec<Complex<f64>>, Vec<Complex<f64>>) {
    let b = poly_complex(z);
    let a = poly_complex(p);
    let b_scaled: Vec<Complex<f64>> = b.into_iter().map(|v| v * k).collect();
    (b_scaled, a)
}

/// 计算多项式系数 poly((x - z1)(x - z2)...)
fn poly_complex(roots: &[Complex<f64>]) -> Vec<Complex<f64>> {
    if roots.is_empty() {
        return vec![Complex::<f64>::new(1.0, 0.0)];
    }

    let mut coeffs = vec![Complex::<f64>::new(1.0, 0.0)];

    for &root in roots {
        let mut new_coeffs = Vec::with_capacity(coeffs.len() + 1);
        new_coeffs.push(-root * coeffs[0]);
        for i in 1..coeffs.len() {
            new_coeffs.push(coeffs[i - 1] - root * coeffs[i]);
        }
        new_coeffs.push(coeffs[coeffs.len() - 1]);
        coeffs = new_coeffs;
    }

    coeffs.reverse();
    coeffs
}