use bone_solver::{
    ConstraintSystem, Line3Handle, NewtonConfig, Point3Handle, Residual, SolverError,
    assemble_jacobian, decompose, evaluate_residuals, jacobian_triplets, residual_norm,
    solve_newton, solve_newton_decomposed, sparsity_pattern,
};
use bone_types::{NewtonDamping, Parameter, ParameterIndex, ResidualIndex, SolverResidual};

fn p(idx: u32) -> ParameterIndex {
    ParameterIndex::new(idx)
}

fn point3(x: u32, y: u32, z: u32) -> Point3Handle {
    Point3Handle {
        x: p(x),
        y: p(y),
        z: p(z),
    }
}

fn parameters(values: &[f64]) -> Vec<Parameter> {
    values.iter().copied().map(Parameter::new).collect()
}

fn on_plane(point: Point3Handle, offset_mm: f64) -> Residual {
    Residual::OnPlane {
        point,
        normal: [0.0, 1.0, 0.0],
        offset_mm,
    }
}

fn coincident(a: Point3Handle, b: Point3Handle) -> Residual {
    Residual::Coincident3(a, b)
}

fn symmetric(a: Point3Handle, b: Point3Handle, axis: Line3Handle) -> Residual {
    Residual::Symmetric3 { a, b, axis }
}

fn x_axis_line() -> Line3Handle {
    Line3Handle {
        a: point3(6, 7, 8),
        b: point3(9, 10, 11),
    }
}

#[test]
fn on_plane_residual_row_and_jacobian_match_hand_derivation() {
    let system = ConstraintSystem::new(
        parameters(&[0.0, 1.5, 3.0]),
        vec![on_plane(point3(0, 1, 2), 0.5)],
    );
    assert_eq!(system.row_count(), 1);
    let values = evaluate_residuals(&system, &[0.0, 1.5, 3.0]);
    assert!((values[0] - (1.5 - 0.5)).abs() < 1e-15);
    let triplets = jacobian_triplets(&system, &[0.0, 1.5, 3.0]);
    let zero = ResidualIndex::new(0);
    assert!(triplets.contains(&(zero, p(1), 1.0)));
}

#[test]
fn pin_solves_in_one_newton_step() {
    let system = ConstraintSystem::new(
        parameters(&[3.0]),
        vec![Residual::Pin {
            param: p(0),
            target: 7.0,
        }],
    );
    let Ok(out) = solve_newton(&system, NewtonConfig::DEFAULT) else {
        panic!("pin is linear, must converge");
    };
    assert!((out[0].value() - 7.0).abs() < 1e-9);
}

#[test]
fn coincident3_is_three_row_residual() {
    let system = ConstraintSystem::new(
        parameters(&[0.0, 0.0, 0.0, 1.0, 2.0, 3.0]),
        vec![coincident(point3(0, 1, 2), point3(3, 4, 5))],
    );
    assert_eq!(system.row_count(), 3);
    let r = evaluate_residuals(&system, &[0.0, 0.0, 0.0, 1.0, 2.0, 3.0]);
    assert_eq!(r.len(), 3);
    assert!((r[0] - (0.0 - 1.0)).abs() < 1e-15);
    assert!((r[1] - (0.0 - 2.0)).abs() < 1e-15);
    assert!((r[2] - (0.0 - 3.0)).abs() < 1e-15);
}

#[test]
fn sparsity_pattern_drops_duplicate_positions() {
    let system = ConstraintSystem::new(
        parameters(&[0.0, 0.0]),
        vec![Residual::Pin {
            param: p(0),
            target: 1.0,
        }],
    );
    let pattern = sparsity_pattern(&system);
    assert_eq!(pattern.rows(), 1);
    assert_eq!(pattern.cols(), 2);
    assert_eq!(pattern.entries().len(), 1);
}

#[test]
fn default_damping_regularises_rank_deficient_jacobian() {
    let system = ConstraintSystem::new(
        parameters(&[0.0, 3.0, 5.0, 7.0, 9.0, 11.0]),
        vec![coincident(point3(0, 1, 2), point3(3, 4, 5))],
    );
    let Ok(out) = solve_newton(&system, NewtonConfig::DEFAULT) else {
        panic!("default damping must regularise rank-deficient J^T J");
    };
    assert!((out[0].value() - out[3].value()).abs() < 1e-9);
    assert!((out[1].value() - out[4].value()).abs() < 1e-9);
    assert!((out[2].value() - out[5].value()).abs() < 1e-9);
}

#[test]
fn zero_damping_on_rank_deficient_jacobian_does_not_silently_succeed() {
    let system = ConstraintSystem::new(
        parameters(&[0.0, 3.0, 5.0, 7.0, 9.0, 11.0]),
        vec![coincident(point3(0, 1, 2), point3(3, 4, 5))],
    );
    let cfg = NewtonConfig {
        damping: NewtonDamping::new(0.0),
        ..NewtonConfig::DEFAULT
    };
    let Err(err) = solve_newton(&system, cfg) else {
        panic!("damping=0 on rank-deficient system must surface an error");
    };
    assert!(
        matches!(
            err,
            SolverError::InvalidSolutionFound { .. } | SolverError::NoSolutionFound { .. }
        ),
        "expected InvalidSolutionFound or NoSolutionFound, got {err:?}"
    );
}

#[test]
fn residual_norm_matches_euclidean_length() {
    let out = evaluate_residuals(
        &ConstraintSystem::new(
            parameters(&[3.0]),
            vec![Residual::Pin {
                param: p(0),
                target: 0.0,
            }],
        ),
        &[3.0],
    );
    let norm = residual_norm(&out);
    assert!((norm.value() - 3.0).abs() < 1e-15);
    let _ = SolverResidual::new(norm.value());
}

#[test]
fn decomposed_singular_index_is_translated_to_global_parameter() {
    let pins: Vec<Residual> = (0..10)
        .map(|i| Residual::Pin {
            param: p(i),
            target: 0.0,
        })
        .collect();
    let residuals: Vec<Residual> = pins
        .into_iter()
        .chain(std::iter::once(coincident(
            point3(10, 11, 12),
            point3(13, 14, 15),
        )))
        .collect();
    let seed = [0.0; 10]
        .iter()
        .copied()
        .chain([0.0, 5.0, 1.0, 7.0, 9.0, 11.0])
        .collect::<Vec<_>>();
    let system = ConstraintSystem::new(parameters(&seed), residuals);
    let decomp = decompose(&system);
    let cfg = NewtonConfig {
        damping: NewtonDamping::new(0.0),
        ..NewtonConfig::DEFAULT
    };
    match solve_newton_decomposed(&system, &decomp, cfg) {
        Err(SolverError::InvalidSolutionFound { at }) => {
            assert!(
                at.as_usize() >= 10,
                "expected global parameter index (>=10), got {at:?}"
            );
        }
        Err(SolverError::NoSolutionFound { .. }) => {}
        other => panic!("expected InvalidSolutionFound or NoSolutionFound, got {other:?}"),
    }
}

#[test]
fn symmetric3_residual_is_zero_when_b_is_reflection_of_a() {
    let state = [0.0, 1.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0];
    let system = ConstraintSystem::new(
        parameters(&state),
        vec![symmetric(point3(0, 1, 2), point3(3, 4, 5), x_axis_line())],
    );
    assert_eq!(system.row_count(), 4);
    let values = evaluate_residuals(&system, &state);
    assert!(values[0].abs() < 1e-15, "midpoint-on-axis x: {}", values[0]);
    assert!(values[1].abs() < 1e-15, "midpoint-on-axis y: {}", values[1]);
    assert!(values[2].abs() < 1e-15, "midpoint-on-axis z: {}", values[2]);
    assert!(
        values[3].abs() < 1e-15,
        "segment normal to axis: {}",
        values[3]
    );
}

#[test]
fn symmetric3_residual_is_nonzero_when_b_drifts_off_reflection() {
    let state = [0.0, 1.0, 0.0, 0.5, -1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0];
    let system = ConstraintSystem::new(
        parameters(&state),
        vec![symmetric(point3(0, 1, 2), point3(3, 4, 5), x_axis_line())],
    );
    let values = evaluate_residuals(&system, &state);
    assert!(
        values.iter().any(|v| v.abs() > 0.0),
        "drift must surface in at least one residual",
    );
}

#[test]
fn symmetric3_solves_to_b_as_reflection_of_a_about_x_axis() {
    let pin = |param, target| Residual::Pin { param, target };
    let system = ConstraintSystem::new(
        parameters(&[0.0, 1.0, 0.0, 0.5, 1.5, 0.3, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]),
        vec![
            pin(p(0), 0.0),
            pin(p(1), 1.0),
            pin(p(2), 0.0),
            pin(p(6), 0.0),
            pin(p(7), 0.0),
            pin(p(8), 0.0),
            pin(p(9), 1.0),
            pin(p(10), 0.0),
            pin(p(11), 0.0),
            symmetric(point3(0, 1, 2), point3(3, 4, 5), x_axis_line()),
        ],
    );
    let Ok(out) = solve_newton(&system, NewtonConfig::DEFAULT) else {
        panic!("symmetric over a fixed axis and source point must converge");
    };
    assert!(
        out[3].value().abs() < 1e-6,
        "b.x = a.x for reflection across the x-axis line: got {}",
        out[3].value()
    );
    assert!(
        (out[4].value() - -1.0).abs() < 1e-6,
        "b.y = -a.y for reflection across the x-axis line: got {}",
        out[4].value()
    );
    assert!(
        out[5].value().abs() < 1e-6,
        "b.z = -a.z for reflection across the x-axis line: got {}",
        out[5].value()
    );
}

#[test]
fn assemble_jacobian_and_triplets_agree_on_dense_shape() {
    let system = ConstraintSystem::new(
        parameters(&[0.0, 0.0, 2.0]),
        vec![on_plane(point3(0, 1, 2), 0.0)],
    );
    let params = vec![0.0, 0.0, 2.0];
    let dense = assemble_jacobian(&system, &params);
    assert_eq!(dense.nrows(), 1);
    assert_eq!(dense.ncols(), 3);
    let triplets = jacobian_triplets(&system, &params);
    let sum: f64 = triplets
        .iter()
        .map(|(r, c, v)| dense[(r.as_usize(), c.as_usize())] - v)
        .sum();
    assert!(sum.abs() < 1e-15);
}
