use bone_types::{ParameterIndex, ResidualIndex};

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Point3Handle {
    pub x: ParameterIndex,
    pub y: ParameterIndex,
    pub z: ParameterIndex,
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Line3Handle {
    pub a: Point3Handle,
    pub b: Point3Handle,
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub enum CurveRadius3 {
    Explicit {
        center: Point3Handle,
        radius: ParameterIndex,
    },
    FromSpoke {
        center: Point3Handle,
        spoke: Point3Handle,
    },
}

impl CurveRadius3 {
    #[must_use]
    pub fn center(self) -> Point3Handle {
        match self {
            Self::Explicit { center, .. } | Self::FromSpoke { center, .. } => center,
        }
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
pub struct Ellipse3Handles {
    pub point: Point3Handle,
    pub center: Point3Handle,
    pub major: Point3Handle,
    pub minor: Point3Handle,
}

#[derive(Clone, Debug, PartialEq)]
pub enum Residual {
    Pin {
        param: ParameterIndex,
        target: f64,
    },
    Coincident3(Point3Handle, Point3Handle),
    CoincidentPointCurve3 {
        point: Point3Handle,
        curve: CurveRadius3,
    },
    CoincidentPointEllipse3(Ellipse3Handles),
    InPlane3 {
        point: Point3Handle,
        origin: Point3Handle,
        normal: [f64; 3],
    },
    Distance3 {
        a: Point3Handle,
        b: Point3Handle,
        value_mm: f64,
    },
    OnPlane {
        point: Point3Handle,
        normal: [f64; 3],
        offset_mm: f64,
    },
    AlongAxis {
        line: Line3Handle,
        basis: [[f64; 3]; 2],
    },
    Perpendicular3(Line3Handle, Line3Handle),
    Parallel3(Line3Handle, Line3Handle),
    Midpoint3 {
        point: Point3Handle,
        line: Line3Handle,
    },
    PointOnLine3 {
        point: Point3Handle,
        line: Line3Handle,
    },
    AngularBetweenLines3 {
        a: Line3Handle,
        b: Line3Handle,
        angle_rad: f64,
    },
    EqualDistance3 {
        center: Point3Handle,
        a: Point3Handle,
        b: Point3Handle,
    },
    OrthogonalSpokes3 {
        center: Point3Handle,
        a: Point3Handle,
        b: Point3Handle,
    },
    RadiusCurve3 {
        curve: CurveRadius3,
        value_mm: f64,
    },
    EqualRadius3(CurveRadius3, CurveRadius3),
    EqualLength3(Line3Handle, Line3Handle),
    TangentLineCurve3 {
        line: Line3Handle,
        curve: CurveRadius3,
    },
    TangentCurveCurve3 {
        a: CurveRadius3,
        b: CurveRadius3,
        internal: bool,
    },
    Symmetric3 {
        a: Point3Handle,
        b: Point3Handle,
        axis: Line3Handle,
    },
}

pub type Triplet = (ResidualIndex, ParameterIndex, f64);

impl Residual {
    #[must_use]
    pub fn rows(&self) -> usize {
        match self {
            Self::Symmetric3 { .. } => 4,
            Self::Coincident3(..)
            | Self::Midpoint3 { .. }
            | Self::Parallel3(..)
            | Self::PointOnLine3 { .. } => 3,
            Self::AlongAxis { .. } => 2,
            _ => 1,
        }
    }

    #[must_use]
    pub fn parameters(&self) -> Vec<ParameterIndex> {
        match *self {
            Self::Pin { param, .. } => vec![param],
            Self::Coincident3(p, q) => point3_params(p)
                .into_iter()
                .chain(point3_params(q))
                .collect(),
            Self::CoincidentPointCurve3 { point, curve } => point3_params(point)
                .into_iter()
                .chain(curve3_params(curve))
                .collect(),
            Self::CoincidentPointEllipse3(e) => point3_params(e.point)
                .into_iter()
                .chain(point3_params(e.center))
                .chain(point3_params(e.major))
                .chain(point3_params(e.minor))
                .collect(),
            Self::InPlane3 { point, origin, .. } => point3_params(point)
                .into_iter()
                .chain(point3_params(origin))
                .collect(),
            Self::Distance3 { a, b, .. } => point3_params(a)
                .into_iter()
                .chain(point3_params(b))
                .collect(),
            Self::OnPlane { point, .. } => point3_params(point),
            Self::AlongAxis { line, .. } => line3_params(line),
            Self::Perpendicular3(a, b)
            | Self::Parallel3(a, b)
            | Self::EqualLength3(a, b)
            | Self::AngularBetweenLines3 { a, b, .. } => {
                line3_params(a).into_iter().chain(line3_params(b)).collect()
            }
            Self::Midpoint3 { point, line } | Self::PointOnLine3 { point, line } => {
                point3_params(point)
                    .into_iter()
                    .chain(line3_params(line))
                    .collect()
            }
            Self::EqualDistance3 { center, a, b } | Self::OrthogonalSpokes3 { center, a, b } => {
                point3_params(center)
                    .into_iter()
                    .chain(point3_params(a))
                    .chain(point3_params(b))
                    .collect()
            }
            Self::RadiusCurve3 { curve, .. } => curve3_params(curve),
            Self::EqualRadius3(a, b) | Self::TangentCurveCurve3 { a, b, .. } => curve3_params(a)
                .into_iter()
                .chain(curve3_params(b))
                .collect(),
            Self::TangentLineCurve3 { line, curve } => line3_params(line)
                .into_iter()
                .chain(curve3_params(curve))
                .collect(),
            Self::Symmetric3 { a, b, axis } => point3_params(a)
                .into_iter()
                .chain(point3_params(b))
                .chain(line3_params(axis))
                .collect(),
        }
    }

    pub fn evaluate(&self, params: &[f64], out: &mut [f64]) {
        match *self {
            Self::Pin { param, target } => {
                out[0] = params[param.as_usize()] - target;
            }
            _ => self.evaluate3(params, out),
        }
    }

    fn evaluate3(&self, params: &[f64], out: &mut [f64]) {
        match *self {
            Self::Coincident3(p, q) => evaluate_coincident3(params, p, q, out),
            Self::CoincidentPointCurve3 { point, curve } => {
                let c = point3_at(params, curve.center());
                let e = (
                    get(params, point.x) - c.0,
                    get(params, point.y) - c.1,
                    get(params, point.z) - c.2,
                );
                out[0] = dot3(e, e) - radius_squared3(params, curve);
            }
            Self::CoincidentPointEllipse3(e) => {
                out[0] = evaluate_point_on_ellipse3(params, e);
            }
            Self::InPlane3 {
                point,
                origin,
                normal,
            } => {
                let e = sub3(point3_at(params, point), point3_at(params, origin));
                out[0] = normal[0] * e.0 + normal[1] * e.1 + normal[2] * e.2;
            }
            Self::Distance3 { a, b, value_mm } => {
                let (dx, dy, dz) = (
                    get(params, b.x) - get(params, a.x),
                    get(params, b.y) - get(params, a.y),
                    get(params, b.z) - get(params, a.z),
                );
                out[0] = dx * dx + dy * dy + dz * dz - value_mm * value_mm;
            }
            Self::OnPlane {
                point,
                normal,
                offset_mm,
            } => {
                out[0] = normal[0] * get(params, point.x)
                    + normal[1] * get(params, point.y)
                    + normal[2] * get(params, point.z)
                    - offset_mm;
            }
            Self::AlongAxis { line, basis } => {
                let (dx, dy, dz) = delta3(params, line);
                let [u, v] = basis;
                out[0] = u[0] * dx + u[1] * dy + u[2] * dz;
                out[1] = v[0] * dx + v[1] * dy + v[2] * dz;
            }
            Self::Perpendicular3(l1, l2) => {
                let (d1x, d1y, d1z) = delta3(params, l1);
                let (d2x, d2y, d2z) = delta3(params, l2);
                out[0] = d1x * d2x + d1y * d2y + d1z * d2z;
            }
            Self::Parallel3(l1, l2) => evaluate_parallel3(params, l1, l2, out),
            Self::Midpoint3 { point, line } => evaluate_midpoint3(params, point, line, out),
            Self::PointOnLine3 { point, line } => {
                evaluate_point_on_line3(params, point, line, out);
            }
            Self::AngularBetweenLines3 { a, b, angle_rad } => {
                out[0] = evaluate_angular_between_lines3(params, a, b, angle_rad);
            }
            Self::EqualDistance3 { center, a, b } => {
                out[0] = evaluate_equal_distance3(params, center, a, b);
            }
            Self::OrthogonalSpokes3 { center, a, b } => {
                out[0] = evaluate_orthogonal_spokes3(params, center, a, b);
            }
            Self::RadiusCurve3 { curve, value_mm } => {
                out[0] = radius_squared3(params, curve) - value_mm * value_mm;
            }
            Self::EqualRadius3(a, b) => {
                out[0] = radius_squared3(params, a) - radius_squared3(params, b);
            }
            Self::EqualLength3(a, b) => {
                let da = delta3(params, a);
                let db = delta3(params, b);
                out[0] = dot3(da, da) - dot3(db, db);
            }
            Self::TangentLineCurve3 { line, curve } => {
                out[0] = evaluate_tangent_line_curve3(params, line, curve);
            }
            Self::TangentCurveCurve3 { a, b, internal } => {
                out[0] = evaluate_tangent_curve_curve3(params, a, b, internal);
            }
            Self::Symmetric3 { a, b, axis } => evaluate_symmetric3(params, a, b, axis, out),
            Self::Pin { .. } => unreachable!("evaluate3 is only reached for 3D residual variants"),
        }
    }

    pub fn jacobian(&self, params: &[f64], row: ResidualIndex, sink: &mut Vec<Triplet>) {
        match *self {
            Self::Pin { param, .. } => sink.push((row, param, 1.0)),
            Self::CoincidentPointCurve3 { point, curve } => {
                let center = curve.center();
                let e = (
                    get(params, point.x) - get(params, center.x),
                    get(params, point.y) - get(params, center.y),
                    get(params, point.z) - get(params, center.z),
                );
                push3(sink, row, point, scale3(e, 2.0));
                push3(sink, row, center, scale3(e, -2.0));
                subtract_radius_squared3_jacobian(params, row, curve, sink);
            }
            Self::CoincidentPointEllipse3(e) => jacobian_point_on_ellipse3(params, row, e, sink),
            Self::RadiusCurve3 { curve, .. } => {
                add_radius_squared3_jacobian(params, row, curve, sink);
            }
            Self::EqualRadius3(a, b) => {
                add_radius_squared3_jacobian(params, row, a, sink);
                subtract_radius_squared3_jacobian(params, row, b, sink);
            }
            Self::EqualLength3(a, b) => {
                let da = delta3(params, a);
                let db = delta3(params, b);
                push3(sink, row, a.a, scale3(da, -2.0));
                push3(sink, row, a.b, scale3(da, 2.0));
                push3(sink, row, b.a, scale3(db, 2.0));
                push3(sink, row, b.b, scale3(db, -2.0));
            }
            Self::TangentLineCurve3 { line, curve } => {
                tangent_line_curve3_jacobian(params, row, line, curve, sink);
            }
            Self::TangentCurveCurve3 { a, b, internal } => {
                tangent_curve_curve3_jacobian(params, row, a, b, internal, sink);
            }
            Self::InPlane3 {
                point,
                origin,
                normal,
            } => {
                push3(sink, row, point, (normal[0], normal[1], normal[2]));
                push3(sink, row, origin, (-normal[0], -normal[1], -normal[2]));
            }
            Self::Symmetric3 { a, b, axis } => jacobian_symmetric3(params, row, a, b, axis, sink),
            Self::Coincident3(p, q) => {
                let row2 = row.next();
                let row3 = row2.next();
                sink.push((row, p.x, 1.0));
                sink.push((row, q.x, -1.0));
                sink.push((row2, p.y, 1.0));
                sink.push((row2, q.y, -1.0));
                sink.push((row3, p.z, 1.0));
                sink.push((row3, q.z, -1.0));
            }
            Self::Distance3 { a, b, .. } => {
                jacobian_distance3(params, row, a, b, sink);
            }
            Self::OnPlane { point, normal, .. } => {
                sink.push((row, point.x, normal[0]));
                sink.push((row, point.y, normal[1]));
                sink.push((row, point.z, normal[2]));
            }
            Self::AlongAxis { line, basis } => {
                jacobian_along_axis(row, line, basis, sink);
            }
            Self::Perpendicular3(l1, l2) => {
                jacobian_perpendicular3(params, row, l1, l2, sink);
            }
            Self::Parallel3(l1, l2) => {
                jacobian_parallel3(params, row, l1, l2, sink);
            }
            Self::Midpoint3 { point, line } => jacobian_midpoint3(row, point, line, sink),
            Self::PointOnLine3 { point, line } => {
                jacobian_point_on_line3(params, row, point, line, sink);
            }
            Self::AngularBetweenLines3 { a, b, angle_rad } => {
                jacobian_angular_between_lines3(params, row, a, b, angle_rad, sink);
            }
            Self::EqualDistance3 { center, a, b } => {
                jacobian_equal_distance3(params, row, center, a, b, sink);
            }
            Self::OrthogonalSpokes3 { center, a, b } => {
                jacobian_orthogonal_spokes3(params, row, center, a, b, sink);
            }
        }
    }
}

struct EllipseTerms {
    dp: (f64, f64, f64),
    dm: (f64, f64, f64),
    dn: (f64, f64, f64),
    a2: f64,
    b2: f64,
    s: f64,
    t: f64,
}

fn ellipse_terms(params: &[f64], e: Ellipse3Handles) -> EllipseTerms {
    let c = point3_at(params, e.center);
    let dp = sub3(point3_at(params, e.point), c);
    let dm = sub3(point3_at(params, e.major), c);
    let dn = sub3(point3_at(params, e.minor), c);
    EllipseTerms {
        dp,
        dm,
        dn,
        a2: dot3(dm, dm),
        b2: dot3(dn, dn),
        s: dot3(dp, dm),
        t: dot3(dp, dn),
    }
}

fn evaluate_point_on_ellipse3(params: &[f64], e: Ellipse3Handles) -> f64 {
    let k = ellipse_terms(params, e);
    k.s * k.s / (k.a2 * k.a2) + k.t * k.t / (k.b2 * k.b2) - 1.0
}

fn jacobian_point_on_ellipse3(
    params: &[f64],
    row: ResidualIndex,
    e: Ellipse3Handles,
    sink: &mut Vec<Triplet>,
) {
    let k = ellipse_terms(params, e);
    let g1 = 2.0 * k.s / (k.a2 * k.a2);
    let g2 = 2.0 * k.t / (k.b2 * k.b2);
    let h1 = -2.0 * k.s * k.s / (k.a2 * k.a2 * k.a2);
    let h2 = -2.0 * k.t * k.t / (k.b2 * k.b2 * k.b2);
    push3(sink, row, e.point, add3(scale3(k.dm, g1), scale3(k.dn, g2)));
    push3(
        sink,
        row,
        e.major,
        add3(scale3(k.dp, g1), scale3(k.dm, 2.0 * h1)),
    );
    push3(
        sink,
        row,
        e.minor,
        add3(scale3(k.dp, g2), scale3(k.dn, 2.0 * h2)),
    );
    push3(
        sink,
        row,
        e.center,
        add3(
            add3(scale3(add3(k.dm, k.dp), -g1), scale3(add3(k.dn, k.dp), -g2)),
            add3(scale3(k.dm, -2.0 * h1), scale3(k.dn, -2.0 * h2)),
        ),
    );
}

fn evaluate_coincident3(params: &[f64], p: Point3Handle, q: Point3Handle, out: &mut [f64]) {
    out[0] = get(params, p.x) - get(params, q.x);
    out[1] = get(params, p.y) - get(params, q.y);
    out[2] = get(params, p.z) - get(params, q.z);
}

fn evaluate_midpoint3(params: &[f64], point: Point3Handle, line: Line3Handle, out: &mut [f64]) {
    out[0] = get(params, point.x) - 0.5 * (get(params, line.a.x) + get(params, line.b.x));
    out[1] = get(params, point.y) - 0.5 * (get(params, line.a.y) + get(params, line.b.y));
    out[2] = get(params, point.z) - 0.5 * (get(params, line.a.z) + get(params, line.b.z));
}

fn evaluate_parallel3(params: &[f64], l1: Line3Handle, l2: Line3Handle, out: &mut [f64]) {
    let (mx, my, mz) = cross3(delta3(params, l1), delta3(params, l2));
    out[0] = mx;
    out[1] = my;
    out[2] = mz;
}

fn evaluate_angular_between_lines3(
    params: &[f64],
    a: Line3Handle,
    b: Line3Handle,
    angle_rad: f64,
) -> f64 {
    let (d1x, d1y, d1z) = delta3(params, a);
    let (d2x, d2y, d2z) = delta3(params, b);
    let dot = d1x * d2x + d1y * d2y + d1z * d2z;
    let g1 = (d1x * d1x + d1y * d1y + d1z * d1z).sqrt();
    let g2 = (d2x * d2x + d2y * d2y + d2z * d2z).sqrt();
    dot - g1 * g2 * angle_rad.cos()
}

fn evaluate_equal_distance3(
    params: &[f64],
    center: Point3Handle,
    a: Point3Handle,
    b: Point3Handle,
) -> f64 {
    let (cx, cy, cz) = (
        get(params, center.x),
        get(params, center.y),
        get(params, center.z),
    );
    let da = sq(get(params, a.x) - cx) + sq(get(params, a.y) - cy) + sq(get(params, a.z) - cz);
    let db = sq(get(params, b.x) - cx) + sq(get(params, b.y) - cy) + sq(get(params, b.z) - cz);
    da - db
}

fn evaluate_orthogonal_spokes3(
    params: &[f64],
    center: Point3Handle,
    a: Point3Handle,
    b: Point3Handle,
) -> f64 {
    let (cx, cy, cz) = (
        get(params, center.x),
        get(params, center.y),
        get(params, center.z),
    );
    (get(params, a.x) - cx) * (get(params, b.x) - cx)
        + (get(params, a.y) - cy) * (get(params, b.y) - cy)
        + (get(params, a.z) - cz) * (get(params, b.z) - cz)
}

fn evaluate_tangent_line_curve3(params: &[f64], line: Line3Handle, curve: CurveRadius3) -> f64 {
    let dir = delta3(params, line);
    let center = point3_at(params, curve.center());
    let anchor = point3_at(params, line.a);
    let radial = (
        center.0 - anchor.0,
        center.1 - anchor.1,
        center.2 - anchor.2,
    );
    let moment = cross3(radial, dir);
    dot3(moment, moment) - radius_squared3(params, curve) * dot3(dir, dir)
}

fn evaluate_tangent_curve_curve3(
    params: &[f64],
    a: CurveRadius3,
    b: CurveRadius3,
    internal: bool,
) -> f64 {
    let ca = point3_at(params, a.center());
    let cb = point3_at(params, b.center());
    let gap = (ca.0 - cb.0, ca.1 - cb.1, ca.2 - cb.2);
    let sign = if internal { -1.0 } else { 1.0 };
    let combo = radius_squared3(params, a).sqrt() + sign * radius_squared3(params, b).sqrt();
    dot3(gap, gap) - combo * combo
}

fn jacobian_angular_between_lines3(
    params: &[f64],
    row: ResidualIndex,
    a: Line3Handle,
    b: Line3Handle,
    angle_rad: f64,
    sink: &mut Vec<Triplet>,
) {
    let (d1x, d1y, d1z) = delta3(params, a);
    let (d2x, d2y, d2z) = delta3(params, b);
    let len_a = (d1x * d1x + d1y * d1y + d1z * d1z).sqrt().max(EPS);
    let len_b = (d2x * d2x + d2y * d2y + d2z * d2z).sqrt().max(EPS);
    let cos_t = angle_rad.cos();
    let grad_d1 = [
        d2x - cos_t * (len_b / len_a) * d1x,
        d2y - cos_t * (len_b / len_a) * d1y,
        d2z - cos_t * (len_b / len_a) * d1z,
    ];
    let grad_d2 = [
        d1x - cos_t * (len_a / len_b) * d2x,
        d1y - cos_t * (len_a / len_b) * d2y,
        d1z - cos_t * (len_a / len_b) * d2z,
    ];
    let line = [a, b];
    let grad = [grad_d1, grad_d2];
    (0..2).for_each(|side| {
        let l = line[side];
        let g = grad[side];
        sink.push((row, l.a.x, -g[0]));
        sink.push((row, l.a.y, -g[1]));
        sink.push((row, l.a.z, -g[2]));
        sink.push((row, l.b.x, g[0]));
        sink.push((row, l.b.y, g[1]));
        sink.push((row, l.b.z, g[2]));
    });
}

fn jacobian_equal_distance3(
    params: &[f64],
    row: ResidualIndex,
    center: Point3Handle,
    a: Point3Handle,
    b: Point3Handle,
    sink: &mut Vec<Triplet>,
) {
    let (cx, cy, cz) = (
        get(params, center.x),
        get(params, center.y),
        get(params, center.z),
    );
    let (ax, ay, az) = (get(params, a.x), get(params, a.y), get(params, a.z));
    let (bx, by, bz) = (get(params, b.x), get(params, b.y), get(params, b.z));
    sink.push((row, center.x, 2.0 * (bx - ax)));
    sink.push((row, center.y, 2.0 * (by - ay)));
    sink.push((row, center.z, 2.0 * (bz - az)));
    sink.push((row, a.x, 2.0 * (ax - cx)));
    sink.push((row, a.y, 2.0 * (ay - cy)));
    sink.push((row, a.z, 2.0 * (az - cz)));
    sink.push((row, b.x, 2.0 * (cx - bx)));
    sink.push((row, b.y, 2.0 * (cy - by)));
    sink.push((row, b.z, 2.0 * (cz - bz)));
}

fn jacobian_orthogonal_spokes3(
    params: &[f64],
    row: ResidualIndex,
    center: Point3Handle,
    a: Point3Handle,
    b: Point3Handle,
    sink: &mut Vec<Triplet>,
) {
    let (cx, cy, cz) = (
        get(params, center.x),
        get(params, center.y),
        get(params, center.z),
    );
    let (ax, ay, az) = (get(params, a.x), get(params, a.y), get(params, a.z));
    let (bx, by, bz) = (get(params, b.x), get(params, b.y), get(params, b.z));
    sink.push((row, a.x, bx - cx));
    sink.push((row, a.y, by - cy));
    sink.push((row, a.z, bz - cz));
    sink.push((row, b.x, ax - cx));
    sink.push((row, b.y, ay - cy));
    sink.push((row, b.z, az - cz));
    sink.push((row, center.x, 2.0 * cx - ax - bx));
    sink.push((row, center.y, 2.0 * cy - ay - by));
    sink.push((row, center.z, 2.0 * cz - az - bz));
}

fn jacobian_midpoint3(
    row: ResidualIndex,
    point: Point3Handle,
    line: Line3Handle,
    sink: &mut Vec<Triplet>,
) {
    let row2 = row.next();
    let row3 = row2.next();
    sink.push((row, point.x, 1.0));
    sink.push((row, line.a.x, -0.5));
    sink.push((row, line.b.x, -0.5));
    sink.push((row2, point.y, 1.0));
    sink.push((row2, line.a.y, -0.5));
    sink.push((row2, line.b.y, -0.5));
    sink.push((row3, point.z, 1.0));
    sink.push((row3, line.a.z, -0.5));
    sink.push((row3, line.b.z, -0.5));
}

fn point_on_line_offset3(
    params: &[f64],
    point: Point3Handle,
    line: Line3Handle,
) -> (f64, f64, f64) {
    sub3(point3_at(params, point), point3_at(params, line.a))
}

fn evaluate_point_on_line3(
    params: &[f64],
    point: Point3Handle,
    line: Line3Handle,
    out: &mut [f64],
) {
    let e = point_on_line_offset3(params, point, line);
    let d = delta3(params, line);
    let (rx, ry, rz) = cross3(e, d);
    out[0] = rx;
    out[1] = ry;
    out[2] = rz;
}

fn jacobian_point_on_line3(
    params: &[f64],
    row: ResidualIndex,
    point: Point3Handle,
    line: Line3Handle,
    sink: &mut Vec<Triplet>,
) {
    let e = point_on_line_offset3(params, point, line);
    let d = delta3(params, line);
    let sd = skew3(d);
    let se = skew3(e);
    let rows = rows3(row);
    push_block3(sink, rows, point, mat_scale(sd, -1.0));
    push_block3(sink, rows, line.b, se);
    push_block3(sink, rows, line.a, mat_sub(sd, se));
}

fn jacobian_distance3(
    params: &[f64],
    row: ResidualIndex,
    a: Point3Handle,
    b: Point3Handle,
    sink: &mut Vec<Triplet>,
) {
    let (dx, dy, dz) = (
        get(params, b.x) - get(params, a.x),
        get(params, b.y) - get(params, a.y),
        get(params, b.z) - get(params, a.z),
    );
    sink.push((row, a.x, -2.0 * dx));
    sink.push((row, a.y, -2.0 * dy));
    sink.push((row, a.z, -2.0 * dz));
    sink.push((row, b.x, 2.0 * dx));
    sink.push((row, b.y, 2.0 * dy));
    sink.push((row, b.z, 2.0 * dz));
}

fn push_dir3(sink: &mut Vec<Triplet>, row: ResidualIndex, line: Line3Handle, g: (f64, f64, f64)) {
    sink.push((row, line.a.x, -g.0));
    sink.push((row, line.a.y, -g.1));
    sink.push((row, line.a.z, -g.2));
    sink.push((row, line.b.x, g.0));
    sink.push((row, line.b.y, g.1));
    sink.push((row, line.b.z, g.2));
}

fn jacobian_along_axis(
    row: ResidualIndex,
    line: Line3Handle,
    basis: [[f64; 3]; 2],
    sink: &mut Vec<Triplet>,
) {
    let [u, v] = basis;
    push_dir3(sink, row, line, (u[0], u[1], u[2]));
    push_dir3(sink, row.next(), line, (v[0], v[1], v[2]));
}

fn jacobian_perpendicular3(
    params: &[f64],
    row: ResidualIndex,
    l1: Line3Handle,
    l2: Line3Handle,
    sink: &mut Vec<Triplet>,
) {
    let d1 = delta3(params, l1);
    let d2 = delta3(params, l2);
    push_dir3(sink, row, l1, d2);
    push_dir3(sink, row, l2, d1);
}

fn jacobian_parallel3(
    params: &[f64],
    row: ResidualIndex,
    l1: Line3Handle,
    l2: Line3Handle,
    sink: &mut Vec<Triplet>,
) {
    let (a1, a2, a3) = delta3(params, l1);
    let (b1, b2, b3) = delta3(params, l2);
    let row1 = row.next();
    let row2 = row1.next();
    push_dir3(sink, row, l1, (0.0, b3, -b2));
    push_dir3(sink, row, l2, (0.0, -a3, a2));
    push_dir3(sink, row1, l1, (-b3, 0.0, b1));
    push_dir3(sink, row1, l2, (a3, 0.0, -a1));
    push_dir3(sink, row2, l1, (b2, -b1, 0.0));
    push_dir3(sink, row2, l2, (-a2, a1, 0.0));
}

const EPS: f64 = 1.0e-12;

fn sq(value: f64) -> f64 {
    value * value
}

fn point3_params(p: Point3Handle) -> Vec<ParameterIndex> {
    vec![p.x, p.y, p.z]
}

fn line3_params(l: Line3Handle) -> Vec<ParameterIndex> {
    vec![l.a.x, l.a.y, l.a.z, l.b.x, l.b.y, l.b.z]
}

fn delta3(params: &[f64], line: Line3Handle) -> (f64, f64, f64) {
    (
        get(params, line.b.x) - get(params, line.a.x),
        get(params, line.b.y) - get(params, line.a.y),
        get(params, line.b.z) - get(params, line.a.z),
    )
}

fn curve3_params(c: CurveRadius3) -> Vec<ParameterIndex> {
    match c {
        CurveRadius3::Explicit { center, radius } => vec![center.x, center.y, center.z, radius],
        CurveRadius3::FromSpoke { center, spoke } => point3_params(center)
            .into_iter()
            .chain(point3_params(spoke))
            .collect(),
    }
}

fn point3_at(params: &[f64], p: Point3Handle) -> (f64, f64, f64) {
    (get(params, p.x), get(params, p.y), get(params, p.z))
}

fn dot3(a: (f64, f64, f64), b: (f64, f64, f64)) -> f64 {
    a.0 * b.0 + a.1 * b.1 + a.2 * b.2
}

fn cross3(a: (f64, f64, f64), b: (f64, f64, f64)) -> (f64, f64, f64) {
    (
        a.1 * b.2 - a.2 * b.1,
        a.2 * b.0 - a.0 * b.2,
        a.0 * b.1 - a.1 * b.0,
    )
}

fn radius_squared3(params: &[f64], curve: CurveRadius3) -> f64 {
    match curve {
        CurveRadius3::Explicit { radius, .. } => {
            let r = get(params, radius);
            r * r
        }
        CurveRadius3::FromSpoke { center, spoke } => {
            let c = point3_at(params, center);
            let s = point3_at(params, spoke);
            sq(s.0 - c.0) + sq(s.1 - c.1) + sq(s.2 - c.2)
        }
    }
}

fn scale_radius_squared3_jacobian(
    params: &[f64],
    row: ResidualIndex,
    curve: CurveRadius3,
    coef: f64,
    sink: &mut Vec<Triplet>,
) {
    if coef == 0.0 {
        return;
    }
    match curve {
        CurveRadius3::Explicit { radius, .. } => {
            sink.push((row, radius, coef * 2.0 * get(params, radius)));
        }
        CurveRadius3::FromSpoke { center, spoke } => {
            let c = point3_at(params, center);
            let s = point3_at(params, spoke);
            let d = (s.0 - c.0, s.1 - c.1, s.2 - c.2);
            sink.push((row, spoke.x, coef * 2.0 * d.0));
            sink.push((row, spoke.y, coef * 2.0 * d.1));
            sink.push((row, spoke.z, coef * 2.0 * d.2));
            sink.push((row, center.x, coef * -2.0 * d.0));
            sink.push((row, center.y, coef * -2.0 * d.1));
            sink.push((row, center.z, coef * -2.0 * d.2));
        }
    }
}

fn add_radius_squared3_jacobian(
    params: &[f64],
    row: ResidualIndex,
    curve: CurveRadius3,
    sink: &mut Vec<Triplet>,
) {
    scale_radius_squared3_jacobian(params, row, curve, 1.0, sink);
}

fn subtract_radius_squared3_jacobian(
    params: &[f64],
    row: ResidualIndex,
    curve: CurveRadius3,
    sink: &mut Vec<Triplet>,
) {
    scale_radius_squared3_jacobian(params, row, curve, -1.0, sink);
}

fn push_radius3_linear_jacobian(
    params: &[f64],
    row: ResidualIndex,
    curve: CurveRadius3,
    coef: f64,
    sink: &mut Vec<Triplet>,
) {
    if coef == 0.0 {
        return;
    }
    match curve {
        CurveRadius3::Explicit { radius, .. } => {
            sink.push((row, radius, coef));
        }
        CurveRadius3::FromSpoke { center, spoke } => {
            let c = point3_at(params, center);
            let s = point3_at(params, spoke);
            let d = (s.0 - c.0, s.1 - c.1, s.2 - c.2);
            let len = (d.0 * d.0 + d.1 * d.1 + d.2 * d.2).sqrt();
            if len == 0.0 {
                return;
            }
            let k = coef / len;
            sink.push((row, spoke.x, k * d.0));
            sink.push((row, spoke.y, k * d.1));
            sink.push((row, spoke.z, k * d.2));
            sink.push((row, center.x, -k * d.0));
            sink.push((row, center.y, -k * d.1));
            sink.push((row, center.z, -k * d.2));
        }
    }
}

fn tangent_line_curve3_jacobian(
    params: &[f64],
    row: ResidualIndex,
    line: Line3Handle,
    curve: CurveRadius3,
    sink: &mut Vec<Triplet>,
) {
    let d = delta3(params, line);
    let c = point3_at(params, curve.center());
    let a0 = point3_at(params, line.a);
    let e = (c.0 - a0.0, c.1 - a0.1, c.2 - a0.2);
    let w = cross3(e, d);
    let len2 = dot3(d, d);
    let r2 = radius_squared3(params, curve);
    let grad_e = scale3(cross3(d, w), 2.0);
    let grad_d = sub3(scale3(cross3(w, e), 2.0), scale3(d, 2.0 * r2));
    let grad_line_a = scale3(add3(grad_e, grad_d), -1.0);
    push3(sink, row, line.a, grad_line_a);
    push3(sink, row, line.b, grad_d);
    push3(sink, row, curve.center(), grad_e);
    scale_radius_squared3_jacobian(params, row, curve, -len2, sink);
}

fn tangent_curve_curve3_jacobian(
    params: &[f64],
    row: ResidualIndex,
    a: CurveRadius3,
    b: CurveRadius3,
    internal: bool,
    sink: &mut Vec<Triplet>,
) {
    let ca = point3_at(params, a.center());
    let cb = point3_at(params, b.center());
    let u = (ca.0 - cb.0, ca.1 - cb.1, ca.2 - cb.2);
    let sign = if internal { -1.0 } else { 1.0 };
    let combo = radius_squared3(params, a).sqrt() + sign * radius_squared3(params, b).sqrt();
    push3(sink, row, a.center(), scale3(u, 2.0));
    push3(sink, row, b.center(), scale3(u, -2.0));
    push_radius3_linear_jacobian(params, row, a, -2.0 * combo, sink);
    push_radius3_linear_jacobian(params, row, b, -2.0 * combo * sign, sink);
}

fn symmetric_offset3(
    params: &[f64],
    a: Point3Handle,
    b: Point3Handle,
    axis: Line3Handle,
) -> (f64, f64, f64) {
    let pa = point3_at(params, a);
    let pb = point3_at(params, b);
    let a0 = point3_at(params, axis.a);
    sub3(
        (
            f64::midpoint(pa.0, pb.0),
            f64::midpoint(pa.1, pb.1),
            f64::midpoint(pa.2, pb.2),
        ),
        a0,
    )
}

fn evaluate_symmetric3(
    params: &[f64],
    a: Point3Handle,
    b: Point3Handle,
    axis: Line3Handle,
    out: &mut [f64],
) {
    let e = symmetric_offset3(params, a, b, axis);
    let d = delta3(params, axis);
    let (cx, cy, cz) = cross3(e, d);
    out[0] = cx;
    out[1] = cy;
    out[2] = cz;
    out[3] = dot3(sub3(point3_at(params, b), point3_at(params, a)), d);
}

fn jacobian_symmetric3(
    params: &[f64],
    row: ResidualIndex,
    a: Point3Handle,
    b: Point3Handle,
    axis: Line3Handle,
    sink: &mut Vec<Triplet>,
) {
    let e = symmetric_offset3(params, a, b, axis);
    let d = delta3(params, axis);
    let sd = skew3(d);
    let se = skew3(e);
    let cross_rows = rows3(row);
    push_block3(sink, cross_rows, a, mat_scale(sd, -0.5));
    push_block3(sink, cross_rows, b, mat_scale(sd, -0.5));
    push_block3(sink, cross_rows, axis.a, mat_sub(sd, se));
    push_block3(sink, cross_rows, axis.b, se);
    let perp = cross_rows[2].next();
    let bma = sub3(point3_at(params, b), point3_at(params, a));
    push3(sink, perp, a, scale3(d, -1.0));
    push3(sink, perp, b, d);
    push3(sink, perp, axis.a, scale3(bma, -1.0));
    push3(sink, perp, axis.b, bma);
}

fn push3(sink: &mut Vec<Triplet>, row: ResidualIndex, p: Point3Handle, g: (f64, f64, f64)) {
    sink.push((row, p.x, g.0));
    sink.push((row, p.y, g.1));
    sink.push((row, p.z, g.2));
}

type Mat3 = [(f64, f64, f64); 3];

fn skew3(v: (f64, f64, f64)) -> Mat3 {
    [(0.0, -v.2, v.1), (v.2, 0.0, -v.0), (-v.1, v.0, 0.0)]
}

fn mat_scale(m: Mat3, k: f64) -> Mat3 {
    [scale3(m[0], k), scale3(m[1], k), scale3(m[2], k)]
}

fn mat_sub(a: Mat3, b: Mat3) -> Mat3 {
    [sub3(a[0], b[0]), sub3(a[1], b[1]), sub3(a[2], b[2])]
}

fn rows3(row: ResidualIndex) -> [ResidualIndex; 3] {
    let r1 = row.next();
    let r2 = r1.next();
    [row, r1, r2]
}

fn push_block3(sink: &mut Vec<Triplet>, rows: [ResidualIndex; 3], p: Point3Handle, block: Mat3) {
    rows.into_iter()
        .zip(block)
        .for_each(|(r, g)| push3(sink, r, p, g));
}

fn add3(a: (f64, f64, f64), b: (f64, f64, f64)) -> (f64, f64, f64) {
    (a.0 + b.0, a.1 + b.1, a.2 + b.2)
}

fn sub3(a: (f64, f64, f64), b: (f64, f64, f64)) -> (f64, f64, f64) {
    (a.0 - b.0, a.1 - b.1, a.2 - b.2)
}

fn scale3(a: (f64, f64, f64), k: f64) -> (f64, f64, f64) {
    (a.0 * k, a.1 * k, a.2 * k)
}

fn get(params: &[f64], idx: ParameterIndex) -> f64 {
    params[idx.as_usize()]
}
