"""Tests encode the structural facts the study depends on."""
import math

import numpy as np

from tensegrity.gait import Robot, pair_actions, single_actions
from tensegrity.model import bar_pairs, build_model, cable_pairs, node_positions


def test_six_bar_icosahedron_topology():
    # A 6-bar tensegrity: 12 nodes, each node on exactly one bar and 4 cables.
    nodes = node_positions(1.0)
    cables = cable_pairs(nodes)
    assert len(nodes) == 12 and len(bar_pairs()) == 6 and len(cables) == 24
    degree = np.bincount(np.array(cables).ravel(), minlength=12)
    assert (degree == 4).all()


def test_bar_length_matches_parameter():
    nodes = node_positions(1.4)
    for a, b in bar_pairs():
        assert math.isclose(np.linalg.norm(nodes[a] - nodes[b]), 1.4)


def test_model_has_one_actuator_per_cable():
    model = build_model()
    assert model.ntendon == 24 and model.nu == 24


def test_cables_are_tension_only():
    # Cables can pull but never push; otherwise "contraction" physics is wrong.
    robot = Robot()
    robot.data.ctrl[:] = 1.0  # rest length longer than every cable -> slack
    robot.run(0.2)
    assert (robot.cable_tension() >= -1e-9).all()
    assert np.allclose(robot.cable_tension(), 0, atol=1e-6)


def test_robot_settles_stably_on_flat_ground():
    robot = Robot()
    x_before = robot.com()[0]
    robot.run(1.0)
    assert abs(robot.com()[0] - x_before) < 0.01
    assert not robot.unstable()


def test_contracting_a_cable_makes_it_roll():
    # The core hypothesis: shifting the CoM by cable contraction causes a roll.
    robot = Robot()
    state = robot.save()
    moves = []
    for action in single_actions():
        robot.load(state)
        moves.append(abs(robot.step_action(action, ratio=0.4)))
    assert max(moves) > 0.15  # at least one cable produces a real step


def test_tilted_gravity_pulls_downhill():
    robot = Robot(slope_deg=20)
    assert robot.model.opt.gravity[0] < 0  # uphill is +x


def test_pair_actions_share_a_node():
    cables = cable_pairs(node_positions(1.0))
    pairs = pair_actions()
    assert len(pairs) == 72
    assert all(set(cables[a]) & set(cables[b]) for a, b in pairs)