//! Motion tracking, clean-room from textbook methods:
//!
//! - each **track point** has a *feature region* (the pattern to follow) and a *search region*
//!   (where to look for it in the next frame), as in After Effects' Tracker;
//! - the pattern is located by **normalized cross-correlation** (Lewis, "Fast Normalized
//!   Cross-Correlation", 1995) searched **coarse to fine over an image pyramid**;
//! - the integer match is refined to **sub-pixel** precision (and, optionally, the feature's
//!   rotation and scale) by **Lucas–Kanade** Gauss–Newton image alignment (Lucas & Kanade 1981;
//!   Baker & Matthews, "Lucas-Kanade 20 Years On", 2004) with a gain/bias photometric model;
//! - **confidence** is the correlation of the template with the matched region, in percent;
//!   when it drops below a threshold the tracker continues, stops, extrapolates the motion or
//!   adapts the feature (re-samples the template), like AE's Motion Tracker Options.
//!
//! Only the pixels around each search region are converted and pre-processed, so tracking cost is
//! independent of the frame size. Points are tracked in parallel and the coarse correlation search
//! is parallel over candidate rows (rayon).
//!
//! Coordinates are *layer* pixels: the frame image's pixel `(i, j)` covers `[i, i+1) × [j, j+1)`
//! after subtracting the frame's `offset` (see [`Frame`]).

pub mod camtrack;
pub mod fit;
pub mod klt;
pub mod mask;
pub mod plane;
pub mod roto;
pub mod solve;
pub mod stabilize;

use effectcraft_raster::Image;
use rayon::prelude::*;
use serde::{Deserialize, Serialize};

pub use plane::{Channel, Plane};
pub use solve::{Homography, angle_and_length, unwrap_degrees};

/// What to do when a frame's confidence is below the threshold.
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub enum ConfidenceAction {
    /// Keep the match anyway.
    Continue,
    /// Stop the analysis before this frame.
    Stop,
    /// Ignore the match and continue the previous motion.
    Extrapolate,
    /// Keep the match and re-sample the feature from this frame.
    #[default]
    Adapt,
}

impl ConfidenceAction {
    pub const ALL: [ConfidenceAction; 4] = [ConfidenceAction::Continue, ConfidenceAction::Stop, ConfidenceAction::Extrapolate, ConfidenceAction::Adapt];
    pub fn label(self) -> &'static str {
        match self {
            ConfidenceAction::Continue => "Continue Tracking",
            ConfidenceAction::Stop => "Stop Tracking",
            ConfidenceAction::Extrapolate => "Extrapolate Motion",
            ConfidenceAction::Adapt => "Adapt Feature",
        }
    }
    pub fn from_name(s: &str) -> Option<ConfidenceAction> {
        let n = s.to_ascii_lowercase().replace([' ', '_', '-'], "");
        ConfidenceAction::ALL.into_iter().find(|a| {
            format!("{a:?}").to_ascii_lowercase() == n
                || a.label().to_ascii_lowercase().replace(' ', "") == n
                || a.label().to_ascii_lowercase().replace(' ', "").starts_with(&n)
        })
    }
}

/// Motion Tracker Options that affect matching.
#[derive(Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(default, rename_all = "camelCase")]
pub struct TrackOptions {
    pub channel: Channel,
    /// Pre-process blur radius in pixels (0 = off).
    pub blur: f64,
    /// Pre-process edge enhancement.
    pub enhance: bool,
    /// Sub-pixel positioning (else whole-pixel matches).
    pub subpixel: bool,
    /// Re-sample the feature on every frame.
    pub adapt_every_frame: bool,
    /// Confidence threshold in percent for `action`.
    pub threshold: f64,
    pub action: ConfidenceAction,
    /// Also estimate each feature's rotation and scale while matching (follows rotating or
    /// zooming features without adapting).
    pub track_shape: bool,
}

impl Default for TrackOptions {
    fn default() -> Self {
        TrackOptions {
            channel: Channel::Luminance,
            blur: 0.0,
            enhance: false,
            subpixel: true,
            adapt_every_frame: false,
            threshold: 80.0,
            action: ConfidenceAction::Adapt,
            track_shape: true,
        }
    }
}

/// One track point's regions (layer pixels).
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct PointSpec {
    pub center: [f64; 2],
    /// Feature region width and height.
    pub feature_size: [f64; 2],
    /// Search region centre relative to the feature centre.
    pub search_offset: [f64; 2],
    /// Search region width and height.
    pub search_size: [f64; 2],
}

/// A frame to track in: layer pixels map to image pixels by adding `offset`.
#[derive(Clone, Copy)]
pub struct Frame<'a> {
    pub img: &'a Image,
    pub offset: [f64; 2],
}

impl<'a> Frame<'a> {
    pub fn new(img: &'a Image) -> Frame<'a> {
        Frame { img, offset: [0.0; 2] }
    }
}

/// How a point's position on a frame was obtained.
#[derive(Clone, Copy, Debug, PartialEq, Eq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub enum Status {
    Tracked,
    /// Tracked, and the feature was re-sampled from this frame.
    Adapted,
    /// Low confidence: the previous motion was continued.
    Extrapolated,
    /// Low confidence with the Stop action: no result for this frame.
    Stopped,
}

/// A point's result on one frame.
#[derive(Clone, Copy, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename_all = "camelCase")]
pub struct PointResult {
    /// Feature centre (layer pixels).
    pub center: [f64; 2],
    /// Match confidence, 0–100 %.
    pub confidence: f64,
    /// Feature rotation (degrees) and scale (1 = start) relative to the start frame (when
    /// `track_shape` is on).
    pub rotation: f64,
    pub scale: f64,
    pub status: Status,
}

/// A template sampled on a unit grid around the feature centre at one pyramid level.
#[derive(Clone, Debug)]
struct Patch {
    r: [i32; 2],
    ch: usize,
    raw: Vec<f32>,
    /// Zero-mean values and their sum of squares.
    zm: Vec<f32>,
    ss: f64,
}

impl Patch {
    fn sample(plane: &Plane, c: [f64; 2], a: [f64; 2], r: [i32; 2]) -> Patch {
        let ch = plane.ch;
        let mut raw = Vec::with_capacity(((2 * r[0] + 1) * (2 * r[1] + 1)) as usize * ch);
        let mut v = [0.0f32; 3];
        for gy in -r[1]..=r[1] {
            for gx in -r[0]..=r[0] {
                let (ux, uy) = (gx as f64, gy as f64);
                plane.sample(c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy, &mut v);
                raw.extend_from_slice(&v[..ch]);
            }
        }
        let mean = raw.iter().map(|x| *x as f64).sum::<f64>() / raw.len().max(1) as f64;
        let zm: Vec<f32> = raw.iter().map(|x| (*x as f64 - mean) as f32).collect();
        let ss = zm.iter().map(|x| (*x as f64) * (*x as f64)).sum();
        Patch { r, ch, raw, zm, ss }
    }
}

/// Normalized cross-correlation of `patch` with `plane` sampled at `c + A u` (A = similarity
/// `[[a0, -a1], [a1, a0]]`). Pure translations share one set of bilinear weights.
fn ncc(plane: &Plane, p: &Patch, c: [f64; 2], a: [f64; 2]) -> f64 {
    if p.ss <= 1e-12 {
        return 0.0;
    }
    let ch = p.ch;
    let (mut s, mut s2, mut st) = (0.0f64, 0.0f64, 0.0f64);
    let identity = (a[0] - 1.0).abs() < 1e-9 && a[1].abs() < 1e-9;
    let fx = c[0] - 0.5;
    let fy = c[1] - 0.5;
    let (bx, by) = (fx.floor(), fy.floor());
    let inside = bx as i64 - p.r[0] as i64 >= 0
        && by as i64 - p.r[1] as i64 >= 0
        && (bx as i64 + p.r[0] as i64 + 1) < plane.w as i64
        && (by as i64 + p.r[1] as i64 + 1) < plane.h as i64;
    if identity && inside {
        let (tx, ty) = ((fx - bx) as f32, (fy - by) as f32);
        let (w00, w10, w01, w11) = ((1.0 - tx) * (1.0 - ty), tx * (1.0 - ty), (1.0 - tx) * ty, tx * ty);
        let row = plane.w * ch;
        let mut k = 0;
        for gy in -p.r[1]..=p.r[1] {
            let y = (by as i64 + gy as i64) as usize;
            let base = y * row;
            for gx in -p.r[0]..=p.r[0] {
                let x = (bx as i64 + gx as i64) as usize;
                let i00 = base + x * ch;
                for cc in 0..ch {
                    let v =
                        w00 * plane.data[i00 + cc] + w10 * plane.data[i00 + ch + cc] + w01 * plane.data[i00 + row + cc] + w11 * plane.data[i00 + row + ch + cc];
                    let v = v as f64;
                    s += v;
                    s2 += v * v;
                    st += v * p.zm[k] as f64;
                    k += 1;
                }
            }
        }
    } else {
        let mut v = [0.0f32; 3];
        let mut k = 0;
        for gy in -p.r[1]..=p.r[1] {
            for gx in -p.r[0]..=p.r[0] {
                let (ux, uy) = (gx as f64, gy as f64);
                plane.sample(c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy, &mut v);
                for cc in 0..ch {
                    let x = v[cc] as f64;
                    s += x;
                    s2 += x * x;
                    st += x * p.zm[k] as f64;
                    k += 1;
                }
            }
        }
    }
    let n = p.zm.len() as f64;
    let var = s2 - s * s / n;
    if var <= 1e-12 {
        return 0.0;
    }
    (st / (var * p.ss).sqrt()).clamp(-1.0, 1.0)
}

/// The pre-processed pixels around one point: a pyramid of a crop. Plane coordinates are image
/// pixels minus `origin`, divided by `2^level`.
struct Crop {
    origin: [f64; 2],
    levels: Vec<Plane>,
    grad0: (Plane, Plane),
}

impl Crop {
    /// Crop image pixels `[c - ext, c + ext]` (image pixel coordinates), pre-processed.
    fn new(frame: &Frame, opts: &TrackOptions, c: [f64; 2], ext: [f64; 2], levels: usize) -> Crop {
        let sigma = opts.blur.max(0.0) / 2.0;
        let margin = (sigma * 3.0).ceil() + 3.0 + if opts.enhance { 6.0 } else { 0.0 } + (1 << levels) as f64;
        let x0 = (c[0] - ext[0] - margin).floor();
        let y0 = (c[1] - ext[1] - margin).floor();
        let w = ((2.0 * (ext[0] + margin)).ceil() as usize + 2).max(4);
        let h = ((2.0 * (ext[1] + margin)).ceil() as usize + 2).max(4);
        let mut p = Plane::from_image(frame.img, opts.channel, x0 as i64, y0 as i64, w, h);
        if sigma > 0.05 {
            p = p.gaussian(sigma);
        }
        if opts.enhance {
            p = p.enhance();
        }
        let grad0 = p.gradients();
        let mut lv = vec![p];
        for _ in 0..levels {
            let d = lv.last().map(Plane::downsample).unwrap_or_default();
            lv.push(d);
        }
        Crop { origin: [x0, y0], levels: lv, grad0 }
    }
    fn to_level(&self, p: [f64; 2], k: usize) -> [f64; 2] {
        let s = (1u32 << k) as f64;
        [(p[0] - self.origin[0]) / s, (p[1] - self.origin[1]) / s]
    }
    fn level_to_image(&self, p: [f64; 2], k: usize) -> [f64; 2] {
        let s = (1u32 << k) as f64;
        [p[0] * s + self.origin[0], p[1] * s + self.origin[1]]
    }
}

/// Per-point tracking state.
#[derive(Clone, Debug)]
struct PointState {
    spec: PointSpec,
    /// Current centre in image pixels.
    center: [f64; 2],
    /// Current similarity `[s cos θ, s sin θ]` relative to the template's frame.
    shape: [f64; 2],
    velocity: [f64; 2],
    templates: Vec<Patch>,
    levels: usize,
}

fn half(v: [f64; 2]) -> [f64; 2] {
    [(v[0] / 2.0).max(1.0), (v[1] / 2.0).max(1.0)]
}

impl PointState {
    fn feature_r(&self) -> [i32; 2] {
        let h = half(self.spec.feature_size);
        [h[0].round().max(2.0) as i32, h[1].round().max(2.0) as i32]
    }
    fn level_r(&self, k: usize) -> [i32; 2] {
        let r = self.feature_r();
        let s = (1 << k) as f64;
        [((r[0] as f64 / s).ceil() as i32).max(2), ((r[1] as f64 / s).ceil() as i32).max(2)]
    }
    /// Search range around the predicted centre (pixels, per axis).
    fn range(&self) -> [f64; 2] {
        let f = half(self.spec.feature_size);
        let s = half(self.spec.search_size);
        [(s[0] - f[0]).max(2.0), (s[1] - f[1]).max(2.0)]
    }
    fn extent(&self) -> f64 {
        let r = self.feature_r();
        let s = (self.shape[0] * self.shape[0] + self.shape[1] * self.shape[1]).sqrt().max(1.0);
        ((r[0] * r[0] + r[1] * r[1]) as f64).sqrt() * s + 2.0
    }

    fn new(spec: PointSpec, frame: &Frame, opts: &TrackOptions) -> PointState {
        let c = [spec.center[0] + frame.offset[0], spec.center[1] + frame.offset[1]];
        let mut st = PointState { spec, center: c, shape: [1.0, 0.0], velocity: [0.0; 2], templates: vec![], levels: 0 };
        let range = st.range();
        let fr = st.feature_r();
        let mut l = 0;
        while l < 4 && range[0].min(range[1]) / (2u32 << l) as f64 >= 3.0 && (fr[0].min(fr[1]) as f64) / (2u32 << l) as f64 >= 3.0 {
            l += 1;
        }
        st.levels = l;
        let e = st.extent();
        let crop = Crop::new(frame, opts, c, [e, e], l);
        st.resample(&crop);
        st
    }

    /// Re-sample the templates from `crop` at the current centre and shape.
    fn resample(&mut self, crop: &Crop) {
        self.templates = (0..=self.levels).map(|k| Patch::sample(&crop.levels[k], crop.to_level(self.center, k), self.shape, self.level_r(k))).collect();
    }

    fn step(&mut self, frame: &Frame, opts: &TrackOptions) -> PointResult {
        let prev = self.center;
        let predicted = [prev[0] + self.velocity[0], prev[1] + self.velocity[1]];
        let sc = [predicted[0] + self.spec.search_offset[0], predicted[1] + self.spec.search_offset[1]];
        let range = self.range();
        let e = self.extent();
        let crop = Crop::new(frame, opts, sc, [range[0] + e, range[1] + e], self.levels);
        let top = self.levels;
        // Coarse: exhaustive correlation over the search range at the top level.
        let s = (1u32 << top) as f64;
        let ctop = crop.to_level(sc, top);
        let (rx, ry) = ((range[0] / s).ceil() as i32, (range[1] / s).ceil() as i32);
        let plane = &crop.levels[top];
        let tpl = &self.templates[top];
        let shape = self.shape;
        let mut cands: Vec<(f64, [f64; 2])> = (-ry..=ry)
            .into_par_iter()
            .flat_map_iter(|j| {
                (-rx..=rx).map(move |i| {
                    let c = [ctop[0] + i as f64, ctop[1] + j as f64];
                    (ncc(plane, tpl, c, shape), c)
                })
            })
            .collect();
        // Best score first; ties (flat or repetitive regions) go to the candidate nearest the
        // prediction so featureless areas don't drift.
        let cp = crop.to_level(predicted, top);
        let d2 = |c: &[f64; 2]| (c[0] - cp[0]).powi(2) + (c[1] - cp[1]).powi(2);
        cands.sort_by(|a, b| if (a.0 - b.0).abs() < 1e-9 { d2(&a.1).total_cmp(&d2(&b.1)) } else { b.0.total_cmp(&a.0) });
        let mut seeds: Vec<[f64; 2]> = vec![];
        for (_, c) in &cands {
            if seeds.iter().all(|q| (q[0] - c[0]).abs() > 1.5 || (q[1] - c[1]).abs() > 1.5) {
                seeds.push(*c);
            }
            if seeds.len() == 3 {
                break;
            }
        }
        // Fine: refine each seed down the pyramid in a 5 × 5 neighbourhood.
        let mut best = (f64::MIN, sc);
        for seed in seeds {
            let mut c = seed;
            let mut score = f64::MIN;
            for k in (0..top).rev() {
                c = [c[0] * 2.0, c[1] * 2.0];
                let (pl, tp) = (&crop.levels[k], &self.templates[k]);
                // Start from the projected match; neighbours must beat it (no drift on ties).
                let mut lb = (ncc(pl, tp, c, shape), c);
                for j in -2..=2 {
                    for i in -2..=2 {
                        if i == 0 && j == 0 {
                            continue;
                        }
                        let q = [c[0] + i as f64, c[1] + j as f64];
                        let v = ncc(pl, tp, q, shape);
                        if v > lb.0 + 1e-9 {
                            lb = (v, q);
                        }
                    }
                }
                (score, c) = lb;
            }
            if top == 0 {
                score = ncc(&crop.levels[0], &self.templates[0], c, shape);
            }
            if score > best.0 + 1e-9 {
                best = (score, crop.level_to_image(c, 0));
            }
        }
        let c0 = crop.to_level(best.1, 0);
        // Sub-pixel (and shape) refinement.
        let (mut c, mut a) = (c0, shape);
        if opts.subpixel {
            let mut ok = false;
            if opts.track_shape
                && let Some((cr, ar)) = lk_refine(&crop, &self.templates[0], c0, shape, true)
            {
                (c, a) = (cr, ar);
                ok = true;
            }
            if !ok {
                if let Some((cr, _)) = lk_refine(&crop, &self.templates[0], c0, shape, false) {
                    c = cr;
                } else {
                    c = parabolic(&crop.levels[0], &self.templates[0], c0, shape);
                }
            }
        }
        // A featureless template can't be matched: stay put.
        if self.templates[0].ss < 1e-6 {
            (c, a) = (crop.to_level(predicted, 0), shape);
        }
        let mut conf = ncc(&crop.levels[0], &self.templates[0], c, a);
        // Keep the coarse match if refinement made it worse.
        let coarse = ncc(&crop.levels[0], &self.templates[0], c0, shape);
        if coarse > conf + 0.02 {
            (c, a, conf) = (c0, shape, coarse);
        }
        let confidence = (conf.max(0.0) * 100.0).min(100.0);
        let found = crop.level_to_image(c, 0);
        let low = confidence < opts.threshold;
        let mut status = Status::Tracked;
        if low {
            match opts.action {
                ConfidenceAction::Stop => {
                    return self.result(prev, frame, confidence, Status::Stopped);
                }
                ConfidenceAction::Extrapolate => {
                    self.center = predicted;
                    return self.result(predicted, frame, confidence, Status::Extrapolated);
                }
                ConfidenceAction::Continue => {}
                ConfidenceAction::Adapt => status = Status::Adapted,
            }
        }
        // Only confident matches predict the next search position.
        self.velocity = if low { [0.0; 2] } else { [found[0] - prev[0], found[1] - prev[1]] };
        self.center = found;
        self.shape = a;
        if opts.adapt_every_frame || status == Status::Adapted {
            status = Status::Adapted;
            self.resample(&crop);
        }
        self.result(found, frame, confidence, status)
    }

    fn result(&self, c: [f64; 2], frame: &Frame, confidence: f64, status: Status) -> PointResult {
        PointResult {
            center: [c[0] - frame.offset[0], c[1] - frame.offset[1]],
            confidence,
            rotation: self.shape[1].atan2(self.shape[0]).to_degrees(),
            scale: (self.shape[0] * self.shape[0] + self.shape[1] * self.shape[1]).sqrt(),
            status,
        }
    }
}

/// Sub-pixel peak from 1-D parabola fits of the correlation around `c`.
fn parabolic(plane: &Plane, p: &Patch, c: [f64; 2], a: [f64; 2]) -> [f64; 2] {
    let f = |dx: f64, dy: f64| ncc(plane, p, [c[0] + dx, c[1] + dy], a);
    let m = f(0.0, 0.0);
    let off = |l: f64, r: f64| {
        let d = l - 2.0 * m + r;
        if d.abs() < 1e-12 { 0.0 } else { (0.5 * (l - r) / d).clamp(-0.5, 0.5) }
    };
    [c[0] + off(f(-1.0, 0.0), f(1.0, 0.0)), c[1] + off(f(0.0, -1.0), f(0.0, 1.0))]
}

/// Forward-additive Lucas–Kanade (Gauss–Newton) alignment of the level-0 template with gain and
/// bias: minimise Σ (I(c + A u) − g T(u) − b)² over the centre `c` and, with `shape`, the
/// similarity `A`. Returns `None` when it diverges.
fn lk_refine(crop: &Crop, p: &Patch, c0: [f64; 2], a0: [f64; 2], shape: bool) -> Option<([f64; 2], [f64; 2])> {
    let plane = &crop.levels[0];
    let (gxp, gyp) = &crop.grad0;
    let ch = p.ch;
    let (mut c, mut a) = (c0, a0);
    let mut g = 1.0f64;
    let mut b;
    // Initial photometric estimate from means/variances.
    {
        let mut v = [0.0f32; 3];
        let (mut si, mut si2, mut st, mut st2, mut n) = (0.0, 0.0, 0.0, 0.0, 0.0);
        let mut k = 0;
        for gy in -p.r[1]..=p.r[1] {
            for gx in -p.r[0]..=p.r[0] {
                let (ux, uy) = (gx as f64, gy as f64);
                plane.sample(c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy, &mut v);
                for cc in 0..ch {
                    let (i, t) = (v[cc] as f64, p.raw[k] as f64);
                    si += i;
                    si2 += i * i;
                    st += t;
                    st2 += t * t;
                    n += 1.0;
                    k += 1;
                }
            }
        }
        let (mi, mt) = (si / n, st / n);
        let (vi, vt) = ((si2 / n - mi * mi).max(0.0), (st2 / n - mt * mt).max(0.0));
        if vt > 1e-12 && vi > 1e-12 {
            g = (vi / vt).sqrt();
        }
        b = mi - g * mt;
    }
    let np = if shape { 6 } else { 4 };
    let (mut v, mut vx, mut vy) = ([0.0f32; 3], [0.0f32; 3], [0.0f32; 3]);
    for _ in 0..30 {
        let mut h = [[0.0f64; 6]; 6];
        let mut rhs = [0.0f64; 6];
        let mut k = 0;
        for gy in -p.r[1]..=p.r[1] {
            for gx in -p.r[0]..=p.r[0] {
                let (ux, uy) = (gx as f64, gy as f64);
                let q = [c[0] + a[0] * ux - a[1] * uy, c[1] + a[1] * ux + a[0] * uy];
                plane.sample(q[0], q[1], &mut v);
                gxp.sample(q[0], q[1], &mut vx);
                gyp.sample(q[0], q[1], &mut vy);
                for cc in 0..ch {
                    let t = p.raw[k] as f64;
                    k += 1;
                    let (ix, iy) = (vx[cc] as f64, vy[cc] as f64);
                    let r = v[cc] as f64 - g * t - b;
                    let j: [f64; 6] = if shape { [ix, iy, ix * ux + iy * uy, -ix * uy + iy * ux, -t, -1.0] } else { [ix, iy, -t, -1.0, 0.0, 0.0] };
                    for m in 0..np {
                        rhs[m] -= j[m] * r;
                        for n in m..np {
                            h[m][n] += j[m] * j[n];
                        }
                    }
                }
            }
        }
        for m in 0..np {
            for n in 0..m {
                h[m][n] = h[n][m];
            }
            h[m][m] *= 1.0 + 1e-6;
            h[m][m] += 1e-9;
        }
        let d = if shape {
            solve::solve_linear(h, rhs)?
        } else {
            let hh = [
                [h[0][0], h[0][1], h[0][2], h[0][3]],
                [h[1][0], h[1][1], h[1][2], h[1][3]],
                [h[2][0], h[2][1], h[2][2], h[2][3]],
                [h[3][0], h[3][1], h[3][2], h[3][3]],
            ];
            let x = solve::solve_linear(hh, [rhs[0], rhs[1], rhs[2], rhs[3]])?;
            [x[0], x[1], 0.0, 0.0, x[2], x[3]]
        };
        // Limit a single step (Gauss-Newton overshoot on poorly textured features).
        let step = (d[0] * d[0] + d[1] * d[1]).sqrt();
        let k = if step > 1.0 { 1.0 / step } else { 1.0 };
        c[0] += d[0] * k;
        c[1] += d[1] * k;
        if shape {
            a[0] += d[2] * k;
            a[1] += d[3] * k;
        }
        g += d[4] * k;
        b += d[5] * k;
        if !(c[0].is_finite() && c[1].is_finite() && a[0].is_finite() && a[1].is_finite()) {
            return None;
        }
        if step < 1e-4 && (!shape || (d[2].abs() + d[3].abs()) < 1e-6) {
            break;
        }
    }
    let moved = ((c[0] - c0[0]).powi(2) + (c[1] - c0[1]).powi(2)).sqrt();
    let s0 = (a0[0] * a0[0] + a0[1] * a0[1]).sqrt();
    let s1 = (a[0] * a[0] + a[1] * a[1]).sqrt();
    if moved > 2.5 || !(0.75..=1.33).contains(&(s1 / s0.max(1e-9))) || g <= 0.0 {
        return None;
    }
    Some((c, a))
}

/// Tracks a set of points frame by frame (the points of one tracker).
#[derive(Clone, Debug)]
pub struct Tracker {
    pub opts: TrackOptions,
    points: Vec<PointState>,
}

impl Tracker {
    /// Start tracking `points` as they appear on `frame` (the start frame).
    pub fn new(opts: TrackOptions, points: &[PointSpec], frame: &Frame) -> Tracker {
        let pts = points.par_iter().map(|s| PointState::new(*s, frame, &opts)).collect();
        Tracker { opts, points: pts }
    }

    /// Track every point into the next frame (in either time direction).
    pub fn step(&mut self, frame: &Frame) -> Vec<PointResult> {
        let opts = &self.opts;
        self.points.par_iter_mut().map(|p| p.step(frame, opts)).collect()
    }

    pub fn len(&self) -> usize {
        self.points.len()
    }
    pub fn is_empty(&self) -> bool {
        self.points.is_empty()
    }
}

#[cfg(test)]
mod tests;