# import pytest
import time

import ut_b2.b2_mock as B2Mock
from ut_b2.common.robot_states import SportApiId, SportMode


def test_init_mock_default():
    """
    This test to ensure the mock starts in damper mode by default
    """
    b2_mock = B2Mock.B2Mock()
    assert (
        b2_mock.state() == SportMode.DAMPING
    )  # This should fail for most mathematical systems


def test_standup_to_jointlock_behavior():
    """
    This test is to ensure that the robot correctly emulate
    the locomotion transition state when standing up
    """
    TRANSITION_TIME_S = 1
    b2_mock = B2Mock.B2Mock(standup_time_s=TRANSITION_TIME_S)

    b2_mock.sport_request(SportApiId.STANDUP)

    assert b2_mock.state() == SportMode.LOCOMOTION, "State not transiting to locomotion"

    time.sleep(TRANSITION_TIME_S * 1.2)

    assert (
        b2_mock.state() == SportMode.JOINT_LOCK
    ), "State did not terminate at Joint Lock"


def test_standup_to_BalanceStand_behavior():
    """
    This test is to ensure that the robot correctly emulate
    the locomotion transition state when standing up to balance stand
    """
    TRANSITION_TIME_S = 1
    b2_mock = B2Mock.B2Mock(standup_time_s=TRANSITION_TIME_S)

    b2_mock.sport_request(SportApiId.BALANCESTAND)

    assert b2_mock.state() == SportMode.LOCOMOTION, "State not transiting to locomotion"

    time.sleep(TRANSITION_TIME_S * 1.2)

    assert (
        b2_mock.state() == SportMode.BALANCE_STAND
    ), "State did not terminate at Balance Stand"


def test_sitdown_from_jointlock_behavior():
    """
    This test is to ensure that the robot correctly emulate
    the locomotion transition state when sitting down
    """
    TRANSITION_TIME_S = 1
    b2_mock = B2Mock.B2Mock(
        init_state=SportMode.JOINT_LOCK, standup_time_s=TRANSITION_TIME_S
    )

    b2_mock.sport_request(SportApiId.STANDDOWN)
    assert b2_mock.state() == SportMode.LOCOMOTION, "State not transiting to locomotion"

    time.sleep(TRANSITION_TIME_S * 1.2)

    assert b2_mock.state() == SportMode.LIE_DOWN, "State not transiting to Lie_Down "

    time.sleep(TRANSITION_TIME_S * 1.2)

    assert b2_mock.state() == SportMode.DAMPING, "State did not terminate at Damping"


def test_sitdown_from_balance():
    """
    This test is to ensure that the robot correctly emulate
    the locomotion transition state when sitting down from balance mode
    """
    TRANSITION_TIME_S = 1
    b2_mock = B2Mock.B2Mock(
        init_state=SportMode.BALANCE_STAND, standup_time_s=TRANSITION_TIME_S
    )

    b2_mock.sport_request(SportApiId.STANDDOWN)

    time.sleep(TRANSITION_TIME_S * 2 * 1.2)

    assert b2_mock.state() == SportMode.DAMPING, "State did not terminate at Damping"


def test_joint_lock_to_balance_stand():

    b2_mock = B2Mock.B2Mock(SportMode.JOINT_LOCK)

    b2_mock.sport_request(SportApiId.BALANCESTAND)

    assert b2_mock.state() == SportMode.BALANCE_STAND