# Load library functions we want
import time
import Diablo
import sys

# Reset test number (be between 0.0 and 1.0)
resetTestNumber = 88.0 / 255.0

# Function to loop motor speeds and check status
def RampAndCheckMotors(startSpeed, endSpeed, stepSpeed, delay):
    if startSpeed < endSpeed:
        speeds = list(range(startSpeed, endSpeed, abs(stepSpeed)))
    else:
        speeds = list(range(startSpeed, endSpeed, -abs(stepSpeed)))
    speeds.append(endSpeed)
    for speed in speeds:
        # Set power
        power = speed / 100.0
        DIABLO1.SetMotors(power)
        DIABLO3.SetMotors(power)
        message = '%3d%% ->' % (speed)
        # Reset the enable flag a few times while waiting
        for i in range(5):
            DIABLO1.SetEnabled(False)
            DIABLO3.SetEnabled(True)
            time.sleep(delay / 5.0)
        # Check status
        power11 = DIABLO1.GetMotor1()
        power12 = DIABLO1.GetMotor2()
        power31 = DIABLO3.GetMotor1()
        power32 = DIABLO3.GetMotor2()
        message += ' [%.3f %.3f %.3f %.3f]' % (power11, power12, power31, power32)
        #enabled1 = DIABLO1.GetEnabled()
        #enabled3 = DIABLO3.GetEnabled()
        #message += ' ENA' if enabled1 else ' DIS'
        #message += ' ENA' if enabled3 else ' DIS'
        #epo1 = DIABLO1.GetEpo()
        #epo3 = DIABLO3.GetEpo()
        #message += ' EPO' if epo1 else ' OK '
        #message += ' EPO' if epo3 else ' OK '
        failsafe1 = DIABLO1.GetCommsFailsafe()
        failsafe3 = DIABLO3.GetCommsFailsafe()
        message += ' FS=T' if failsafe1 else ' FS=F'
        message += ' FS=T' if failsafe3 else ' FS=F'
        encMoveMode1 = DIABLO1.GetEncoderMoveMode()
        encMoveMode3 = DIABLO3.GetEncoderMoveMode()
        message += ' ENC=T' if encMoveMode1 else ' ENC=F'
        message += ' ENC=T' if encMoveMode3 else ' ENC=F'
        encSpeed1 = DIABLO1.GetEncoderSpeed()
        encSpeed3 = DIABLO3.GetEncoderSpeed()
        message += ' OK ' if encSpeed1 == resetTestNumber else ' RST'
        message += ' OK ' if encSpeed3 == resetTestNumber else ' RST'
        # Report
        print('  ' + message)

print('Setup Diablo 1')
DIABLO1 = Diablo.Diablo()
DIABLO1.i2cAddress = 0x31
DIABLO1.Init()
if not DIABLO1.foundChip:
    print('Did not find Diablo 1!')
    sys.exit()
DIABLO1.SetEpoIgnore(False)
DIABLO1.SetEncoderMoveMode(False)
DIABLO1.SetEncoderSpeed(resetTestNumber)

print('Setup Diablo 3')
DIABLO3 = Diablo.Diablo()
DIABLO3.i2cAddress = 0x33
DIABLO3.Init()
if not DIABLO3.foundChip:
    print('Did not find Diablo 3!')
    sys.exit()
DIABLO3.SetEpoIgnore(False)
DIABLO3.SetEncoderMoveMode(False)
DIABLO3.SetEncoderSpeed(resetTestNumber)

print('Clear any errors for Diablo 1')
DIABLO1.SetEnabled(False)
DIABLO1.SetCommsFailsafe(False)
DIABLO1.ResetEpo()
DIABLO1.SetMotors(0)
DIABLO1.SetEnabled(True)

print('Clear any errors for Diablo 3')
DIABLO3.SetEnabled(False)
DIABLO3.SetCommsFailsafe(False)
DIABLO3.ResetEpo()
DIABLO3.SetMotors(0)
DIABLO3.SetEnabled(True)

try:
    print('Ramping up speeds')
    RampAndCheckMotors(2, 100, 2, 1)

    print('Ramping down speeds')
    RampAndCheckMotors(98, 0, 2, 1)
finally:
    print('Ensuring motors are off')
    DIABLO1.SetMotors(0)
    DIABLO3.SetMotors(0)