"""Tests for the BFGS fallback solver."""

import math

import pytest
from kindred_solver.bfgs import bfgs_solve
from kindred_solver.expr import Const, Var
from kindred_solver.params import ParamTable


class TestBFGSBasic:
    def test_single_linear(self):
        """Solve x - 3 = 0."""
        pt = ParamTable()
        x = pt.add("x", 0.0)
        assert bfgs_solve([x - Const(3.0)], pt) is True
        assert abs(pt.get_value("x") - 3.0) < 1e-8

    def test_single_quadratic(self):
        """Solve x^2 - 4 = 0 from x=1 → x=2."""
        pt = ParamTable()
        x = pt.add("x", 1.0)
        assert bfgs_solve([x * x - Const(4.0)], pt) is True
        assert abs(pt.get_value("x") - 2.0) < 1e-8

    def test_two_variables(self):
        """Solve x + y = 5, x - y = 1."""
        pt = ParamTable()
        x = pt.add("x", 0.0)
        y = pt.add("y", 0.0)
        assert bfgs_solve([x + y - Const(5.0), x - y - Const(1.0)], pt) is True
        assert abs(pt.get_value("x") - 3.0) < 1e-8
        assert abs(pt.get_value("y") - 2.0) < 1e-8

    def test_empty_system(self):
        pt = ParamTable()
        assert bfgs_solve([], pt) is True

    def test_with_quat_renorm(self):
        """Quaternion re-normalization during BFGS."""
        pt = ParamTable()
        qw = pt.add("qw", 0.9)
        qx = pt.add("qx", 0.1)
        qy = pt.add("qy", 0.1)
        qz = pt.add("qz", 0.1)
        r = qw * qw + qx * qx + qy * qy + qz * qz - Const(1.0)
        groups = [("qw", "qx", "qy", "qz")]
        assert bfgs_solve([r], pt, quat_groups=groups) is True
        w, x, y, z = (pt.get_value(n) for n in ["qw", "qx", "qy", "qz"])
        norm = math.sqrt(w**2 + x**2 + y**2 + z**2)
        assert abs(norm - 1.0) < 1e-8


class TestBFGSGeometric:
    def test_distance_constraint(self):
        """x^2 - 25 = 0 from x=3 → x=5."""
        pt = ParamTable()
        x = pt.add("x", 3.0)
        assert bfgs_solve([x * x - Const(25.0)], pt) is True
        assert abs(pt.get_value("x") - 5.0) < 1e-8

    def test_difficult_initial_guess(self):
        """BFGS should handle worse initial guesses than Newton."""
        pt = ParamTable()
        x = pt.add("x", 100.0)
        y = pt.add("y", -50.0)
        residuals = [x + y - Const(5.0), x - y - Const(1.0)]
        assert bfgs_solve(residuals, pt) is True
        assert abs(pt.get_value("x") - 3.0) < 1e-6
        assert abs(pt.get_value("y") - 2.0) < 1e-6