import time

import pytest

from test_helpers import StartTimeoutError

def move_to_low_limit_switch(motor):

    low_limit = motor.limits()[0]
    motor.move_and_wait(low_limit)
    assert motor.at_target()
    # Field is currently not set in our drivers
    #assert motor.read_field('lowlimitswitch') == 1
    assert not motor.has_error()


def move_to_high_limit_switch(motor):

    low_limit = motor.limits()[1]
    motor.move_and_wait(low_limit)
    assert motor.at_target()
    # Field is currently not set in our drivers
    #assert motor.read_field('highlimitswitch') == 1
    assert not motor.has_error()


def move_while_move(motor, first_target, second_target, time_first_target):
    """
    Start moving to a position which is sufficiently far away, then interrupt
    this move command with another move command. This function assumes a
    sufficiently large distance between the current motor position and 
    first_target so it has enough time to issue the second move command.
    """
    motor.write_field('writepv', first_target)
    time.sleep(time_first_target)
    motor.move_and_wait(second_target)
    assert motor.at_target(second_target)
    assert not motor.has_error()

def stop_then_move(motor, target):
    motor.write_field('stop', 1)
    motor.wait_for_done()
    motor.move_and_wait(target)


def stop(motor, target):
    """
    Stop an motor while it is moving to a target. This function assumes a
    sufficiently large distance between the current motor position and target.
    """
    motor.write_field('writepv', target)
    time.sleep(1)
    motor.write_field('stop', 1)
    motor.wait_for_done()
    assert not motor.at_target(target)
    assert not motor.has_error()


def invalid_target(motor, target):
    current_pos = motor.read_field('readpv')
    try:
        motor.move_and_wait(target)
        pytest.fail('move_and_wait should throw a StartTimeoutError error')
    except StartTimeoutError:
        assert motor.at_target(current_pos)


def targets_outside_limits(motor):
    """
    Try to move the motor outside its limits
    """
    invalid_target(motor, motor.read_field('lowlimit') - 10)
    invalid_target(motor, motor.read_field('highlimit') + 10)