__author__ 	= "Samson Danziger"
__license__ = "GPLv2"


# import statements
from sr.robot import OUTPUT, INPUT, INPUT_PULLUP
from tests.helper import *
from tests.useful_functions import *
import time

def confirm():
    print("Press the button to confirm that this happened.")

def print_start():
    print("=== STARTING TESTS ===")

def print_done():
    print("=== TESTS COMPLETE ===")
    print()

# default test class
class Test(object):
    
    result = False;
    
    def setup(self):
        pass
    
    def run(self):
        pass

# main test classes
class PowerBoardTest(Test):
    
    def __init__(self, robot):
        self.robot = robot
        self.setup()
        
    def __str__(self):
        return "Power Board Test"
    
    def setup(self):
        pass
    
    def run(self):
        print_start()
        print("Checking battery voltage...")
        self.result = assertTrue(self.robot.power.battery.voltage <= 12.6, \
            "Battery voltage is too high.")
        self.result = assertTrue(10.2 <= self.robot.power.battery.voltage, \
            "Battery voltage is too low.")
            
        print("Checking battery current...")
        self.result = assertTrue(self.robot.power.battery.current <= 40, \
            "Battery current is too high.")
        self.result = assertTrue(35 <= self.robot.power.battery.current, \
            "Battery current is probably too low.")
        
        print("Beeping for 1s in C")
        self.robot.power.beep(1000, note='c') 
        confirm()
        self.result = assertSuccess(self.robot.ruggeduinos[0], "The buzzer should have beeped for 1s in C.")
        print_done()


class RuggeduinoTest(Test):
    
    #analogue_pins = [A0, A1, A2, A3, A4, A5]
    
    def __init__(self, robot):
        self.robot = robot
        self.board0 = robot.ruggeduinos[0]
        if robot.ruggeduinos[1]:
            self.board1 = robot.ruggeduinos[1]
        self.setup()
    
    def __str__(self):
        return "Ruggeduino Test"
    
    def setup(self):
        # set all digital pins as output except 13
        for i in range(2, 13):
            self.board0.pin_mode(i, OUTPUT)
            if self.board1:
                self.board1.pin_mode(i, OUTPUT)
        # set all analogue pins as input
        #for j in self.analogue_pins:
        #    self.board0.pin_mode(j, INPUT)
        #    if self.board1:
        #        self.board1.pin_mode(j, INPUT)
        self.board0.pin_mode(13, INPUT)
        if self.board1:
            self.board1.pin_mode(13, OUTPUT)
    
    def _test_single_board(self, board, include_pin_13=0):
        # test that all digital output work
        print("The led should flash twice quickly, 11 times.")
        for i in range(2, 13+include_pin_13):
            flash_led(board, i, 200, 50, 2)
            time.sleep(2)
        confirm()
        self.result = assertSuccess(self.board0, message="The led should have flashed twice quickly, 11 times. " +\
            "Check each of the outputs individually.")
            
    
    def run(self):
        print_start()
        print("Testing ruggeduino board 0...")
        self._test_single_board(self.board0)
        if self.board1:
            print("Testing ruggeduino board 1...")
            self._test_single_board(self.board1, include_pin_13=1)
        print("In the future analogue pins will also be tested.")
        print_done()

class MotorBoardTest(Test):
    
    def __init__(self, robot):
        self.robot = robot
        self.ruggeduino = robot.ruggeduinos[0]
        self.setup()
        
    def __str__(self):
        return "Motor Board Test"

    def setup(self):
        self.board0 = self.robot.motors[0]
        if self.robot.motors[1]:
            self.board1 = self.robot.motors[1]
    
    def run(self):
        print_start()
        print("Testing motor board 0...")
        print("Make sure there is a motor in M0.")
        time.sleep(5) # give time for that to happen
        print("Running M0 for 3s @ 20% without brakes...")
        test_motor(self.board0.m0, 3, 20)
        confirm()
        self.result = assertSuccess(self.ruggeduino, "M0 on board 0 should have spun @ 20% for 3s, then coasted to a halt.") 
        print("Running M0 for 3s @ 20% with brakes...")
        test_motor(self.board0.m0, 3, 20, braking=True)
        confirm()
        self.result = assertSuccess(self.ruggeduino, "M0 on board 0 should have spun @ 20% for 3s, then stopped suddenly.")

        print("Running M1 for 3s @ 20% without brakes...")
        test_motor(self.board0.m1, 3, 20)
        confirm()
        self.result = assertSuccess(self.ruggeduino, "M1 on board 0 should have spun @ 20% for 3s, then coasted to a halt.") 
        print("Running M1 for 3s @ 20% with brakes...")
        test_motor(self.board0.m0, 3, 20, braking=True)
        confirm()
        self.result = assertSuccess(self.ruggeduino, "M1 on board 0 should have spun @ 20% for 3s, then stopped suddenly.")

        if self.board1:
            print("Testing motor board 1...")
            print("Make sure there is a motor in M0.")
            time.sleep(5) # give time for that to happen
            print("Running M0 for 3s @ 20% without brakes...")
            test_motor(self.board1.m0, 3, 20)
            confirm()
            self.result = assertSuccess(self.ruggeduino, "M0 on board 1 should have spun @ 20% for 3s, then coasted to a halt.") 
            print("Running M0 for 3s @ 20% with brakes...")
            test_motor(self.board1.m0, 3, 20, braking=True)
            confirm()
            self.result = assertSuccess(self.ruggeduino, "M0 on board 1 should have spun @ 20% for 3s, then stopped suddenly.")

            print("Running M1 for 3s @ 20% without brakes...")
            test_motor(self.board1.m1, 3, 20)
            confirm()
            self.result = assertSuccess(self.ruggeduino, "M1 on board 1 should have spun @ 20% for 3s, then coasted to a halt.") 
            print("Running M1 for 3s @ 20% with brakes...")
            test_motor(self.board1.m0, 3, 20, braking=True)
            confirm()
            self.result = assertSuccess(self.ruggeduino, "M1 on board 1 should have spun @ 20% for 3s, then stopped suddenly.")
        print_done()


    
class ServoBoardTest(Test):
    
    def __init__(self, robot):
        self.robot = robot
        self.setup()
    
    def __str__(self):
        return "Servo Board Test"
    
    def setup(self):
        self.board0 = self.robot.servos[0]
        if self.robot.servos[1]:
            self.board1 = self.robot.servos[1]
        self.ruggeduino = self.robot.ruggeduinos[0]

    def _run_single_board_test(self, board, board_number, wait_time=5):
        print("Testing servo board {}...".format(board_number))
        for s in range(12):
            print("Testing output {}, make sure a servo is plugged in.".format(s))
            time.sleep(wait_time)
            print("Moving servo on output {}.".format(s))
            test_servo(board[s])
            confirm()
            self.result = assertSuccess(self.ruggeduino, "Servo on output {} should have moved.".format(s))

    
    def run(self):
        print_start()
        self._run_single_board_test(self.board0, 0)
        if self.board1:
            self._run_single_board_test(self.board1, 0)
        print_done()
