#!/usr/bin/env python
# -*- coding: utf-8 -*-

import sys, config, math, time
from ur5controller import DemoController


def check_edges(controller):
    controller.movel(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
    print 'Right edge.'     
    dummy = raw_input('Press any key to continue...')

    controller.movel(config.R_MIN, 0, config.Z_MIN)
    print 'Middle.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

    controller.movel(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
    print 'Left edge.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

def test_movec(controller):
    controller.movec(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
    print 'Right edge.'     
    dummy = raw_input('Press any key to continue...')

    controller.movec(config.R_MIN, 0, config.Z_MIN)
    print 'Middle.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

    controller.movec(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
    print 'Left edge.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

def test_movec_hax(controller):
    controller.movec_hax(config.R_MIN, config.TABLE_EDGE_RIGHT, config.Z_MIN)
    print 'Right edge.'     
    dummy = raw_input('Press any key to continue...')

    controller.movec_hax(config.R_MIN, 0, config.Z_MIN)
    print 'Middle.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

    controller.movec_hax(config.R_MIN, config.TABLE_EDGE_LEFT, config.Z_MIN)
    print 'Left edge.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')


if __name__ == '__main__':
    controller = None
    try:
        controller = DemoController(config)
        #check_edges(controller)
        test_movec_hax(controller)
    except Exception, e:
        print e

    time.sleep(1)
    if controller:
        controller.cleanup()
    sys.exit(1)