#!/usr/bin/env python
"""
A script to controll the NXT with a Wiimote.
You need to replace the MAC addresses at the bottom of the script
if you want it to work with your NXT and Wiimote.

author: Jan-Klaas Kollhof
last changed by: $LastChangedBy$
last changed date: $Date$
revision: $Revision$
"""
from Wiimote import Wiimote, buttonmap
from nxt.motor import *
import nxt.bluesock
import time



class MotorW(Motor):
    def __init__(self, brick, port):
        Motor.__init__(self, brick, port)

    def set_speed(self, power, cutoff=30):
        self.power = power
        
        if (self.power > 0 and self.power < cutoff) or (self.power<0 and self.power > - cutoff):
            self.mode = MODE_IDLE
            self.run_state = RUN_STATE_IDLE
        else:
            self.mode = MODE_MOTOR_ON
            self.run_state = RUN_STATE_RUNNING
        self.set_output_state()

    def turn(self, deg):
        self.mode = MODE_MOTOR_ON
        self.run_state = RUN_STATE_RUNNING
        self.reset_position(0)
        if deg < 0:
            self.power = -80
            deg = - deg
        else:
            self.power = 80
            
        self.tacho_limit = deg
        self.set_output_state()
         



def getMotors(addr):
    print "connecting to NXT"
    sock = nxt.bluesock.BlueSock(addr)
    brick = sock.connect()
    m_a = MotorW(brick, PORT_A)
    m_b = MotorW(brick, PORT_B)
    m_c = MotorW(brick, PORT_C)
    return m_a,m_b, m_c

def main(wiimoteAddr, nxtAddr):
    print "connecting to Wii"
    w=Wiimote(wiimoteAddr, 0)
    w.connect()
    w.enable_force()
    last = time.time()

    m_a,m_b,m_c = getMotors(nxtAddr)

    print "ready"
    
    claw = 0
    while True:
        w._getpacket()

        if w.buttonmask & buttonmap['A']:
            m_b.set_speed(0)
            m_c.set_speed(0)
            break
        
        if w.buttonmask & buttonmap['1']:
            claw = 1

        if w.buttonmask & buttonmap['2']:
            claw = -1
            
        speed  =  w.force_gx() / 10.0

        #the whole -/+ abs() of the speeds can probably be simplified
        #it is all just simple math, but it was late and
        #I couldn't be bothered to think about it.
        if speed < 0:
            reverse = True
        else:
            reverse = False

        speed = abs(speed)
        
        if speed > 1:
            speed = 1

        speed_l = speed_r = speed

        slowdown = w.force_gy() / 26.0
        if slowdown < 0:
            speed_r = speed - speed * abs(slowdown)
        elif slowdown > 0:
            speed_l = speed - speed * abs(slowdown)

        if reverse:
            speed_l = - speed_l
            speed_r = -speed_r
            
        curr = time.time()
        if curr - last > 0.1:
            if claw == -1:
                m_a.turn(45)
            elif claw == 1:
                m_a.turn(-45)
            claw = 0
            m_b.set_speed(speed_r * 100)
            m_c.set_speed(speed_l * 100)
            #print speed, slowdown, speed_l, speed_r
            last = curr


if __name__ == "__main__":
    #replace the MAC addresses with your own
    main(wiimoteAddr="00:19:FD:C1:AF:C4",
                nxtAddr="00:16:53:05:7A:1E")