#!/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.THETA_MAX, 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.THETA_MIN, 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.THETA_MAX, 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.THETA_MIN, 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.THETA_MAX, 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.THETA_MIN, config.Z_MIN)
    print 'Left edge.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

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

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

    # NOTE: current R_MIN+0.1=0.7 is where the tooltip is at the edge
    controller.movec_hax(config.R_MIN+0.1, config.THETA_MIN, config.Z_MIN+0.1)
    print 'Left edge.'
    time.sleep(1)
    dummy = raw_input('Press any key to continue...')

def check_theta_edges(controller):
    controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN)
    controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_RIGHT, config.Z_MIN)
    print 'Theta edge right.'     
    dummy = raw_input('Press any key to continue...')

    controller.movec_hax(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN)
    controller.movel(config.R_LVL0 - config.BLOCK_DIM, config.THETA_EDGE_LEFT, config.Z_MIN)
    print 'Theta edge left.'     
    dummy = raw_input('Press any key to continue...')

def test_pick_place(controller):
    dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap
    # #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 #small gap
    #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 - math.pi/200#no gap

    # Initial: 1x2 left side
    #speed = 0.7
    speed = 0.05

    moves = []
    moves += controller.pick_block(1, config.THETA_EDGE_LEFT, 0, speed)
    moves += controller.place_block(0, dtheta_lvl0/2, 0, speed)
    moves += controller.pick_block(0, config.THETA_EDGE_LEFT, 0, speed)
    moves += controller.place_block(0, -dtheta_lvl0/2, 0, speed)
    controller.movels(moves, wait=True)
    moves.reverse()
    controller.movels(moves, wait=True)
    # Shit is smooth!


def pick_place_small_pyramid(controller):
    dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 + math.pi/64 #medium gap
    #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 #small gap
    #dtheta_lvl0 = config.BLOCK_DIM / config.R_LVL0 - math.pi/200#no gap

    # Initial: 2x2 on each side


    controller.blocklvl2pose(r_lvl, theta, z_lvl) + [acc, vel, rad]

    controller.pick_block(1, config.THETA_EDGE_LEFT, 1) # Pick left
    controller.place_block(0, -0.5*dtheta_lvl0, 0)
    controller.pick_block(1, config.THETA_EDGE_RIGHT, 1) # pick right
    controller.place_block(0, 0.5*dtheta_lvl0, 0)

    controller.pick_block(0, config.THETA_EDGE_LEFT, 1) # Pick left
    controller.place_block(0, -1.5*dtheta_lvl0, 0)
    controller.pick_block(0, config.THETA_EDGE_RIGHT, 1) # pick right
    controller.place_block(0, 1.5*dtheta_lvl0, 0) # foundation complete

    controller.pick_block(1, config.THETA_EDGE_LEFT, 0) # Pick left
    controller.place_block(0, 0, 1)
    controller.pick_block(1, config.THETA_EDGE_RIGHT, 0) # pick right
    controller.place_block(0, dtheta_lvl0, 1)
    controller.pick_block(0, config.THETA_EDGE_LEFT, 0) # Pick left
    controller.place_block(0, -dtheta_lvl0, 1)

    controller.pick_block(0, config.THETA_EDGE_RIGHT, 0) # pick right
    controller.place_block(0, 0, 2)


if __name__ == '__main__':
    controller = None
    try:
        controller = DemoController(config)
        #controller.movel(config.R_LVL0, 0, config.Z_MIN + config.BLOCK_DIM)
        #check_edges(controller)
        #test_movec_hax(controller)
        #check_theta_edges(controller)
        test_pick_place(controller)
        #pick_place_small_pyramid(controller)

        # NEXT: test radial params and arbitrary paths.
    except Exception, e:
        print e

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