# Imports
import webiopi
import math 
from RPIO import PWM

# Retrieve GPIO lib
GPIO = webiopi.GPIO

# -------------------------------------------------- #
# Constants definition                               #
# -------------------------------------------------- #

# Left motor GPIOs
L1=23  # H-Bridge 1
L2=24 # H-Bridge 2
LS=25 # H-Bridge 1,2EN

# Right motor GPIOs
R1=9 # H-Bridge 3
R2=10 # H-Bridge 4
#RS=11  H-Bridge 3,4EN

FS=11

servo = PWM.Servo()
SERVOY  = 8
SERVOX  = 18
SERVOZ  = 7
global angle_y
global angle_x
global angle_z
angle_y = 1500
angle_x = 1500
angle_z = 1500


# -------------------------------------------------- #
# Convenient PWM Function                            #
# -------------------------------------------------- #

# Set the speed of two motors
def set_speed(speed):
    GPIO.pulseRatio(LS, speed)
    

# -------------------------------------------------- #
# Left Motor Functions                               #
# -------------------------------------------------- #

def left_stop():
    GPIO.output(L1, GPIO.LOW)
    GPIO.output(L2, GPIO.LOW)

def left_forward():
    GPIO.output(L1, GPIO.HIGH)
    GPIO.output(L2, GPIO.LOW)

def left_backward():
    GPIO.output(L1, GPIO.LOW)
    GPIO.output(L2, GPIO.HIGH)

# -------------------------------------------------- #
# Right Motor Functions                              #
# -------------------------------------------------- #
def right_stop():
    GPIO.output(R1, GPIO.LOW)
    GPIO.output(R2, GPIO.LOW)

def right_forward():
    GPIO.output(R1, GPIO.HIGH)
    GPIO.output(R2, GPIO.LOW)

def right_backward():
    GPIO.output(R1, GPIO.LOW)
    GPIO.output(R2, GPIO.HIGH)

# -------------------------------------------------- #
# servo Y control                                    #
# -------------------------------------------------- #
def servo_y_aum():
	global angle_y
	angle_y += 50
	if angle_y > 2000:
		angle_y = 2000
	servo.set_servo(SERVOY, angle_y)

def servo_y_dim():
	global angle_y
	angle_y -= 50
	if angle_y < 1000:
		angle_y = 1000
	servo.set_servo(SERVOY, angle_y)


# -------------------------------------------------- #
# servo X control                                    #
# -------------------------------------------------- #
def servo_x_aum():
	global angle_x
	angle_x += 50
	if angle_x > 2000:
		angle_x = 2000
	servo.set_servo(SERVOX, angle_x)

def servo_x_dim():
	global angle_x
	angle_x -= 50
	if angle_x < 1000:
		angle_x = 1000
	servo.set_servo(SERVOX, angle_x)

# -------------------------------------------------- #
# servo Z control                                    #
# -------------------------------------------------- #
def servo_z_aum():
	global angle_z
	angle_z += 50
	if angle_z > 2000:
		angle_z = 2000
	servo.set_servo(SERVOZ, angle_z)

def servo_z_dim():
	global angle_z
	angle_z -= 50
	if angle_z < 1000:
		angle_z = 1000
	servo.set_servo(SERVOZ, angle_z)
	
# -------------------------------------------------- #
# Macro definition part                              #
# -------------------------------------------------- #

def go_forward():
    left_forward()
    right_forward()

def go_backward():
    left_backward()
    right_backward()

def turn_left():
    left_backward()
    

def turn_right():
    left_forward()
    

def stop():
    left_stop()
    right_stop()

def servo_center():
	global angle_y
	angle_y = 1500
	global angle_x
	angle_x = 1500
	servo.set_servo(SERVOY, angle_y)
	servo.set_servo(SERVOX, angle_x)

def servo_centerX():
	
	global angle_x
	angle_x = 1500
	servo.set_servo(SERVOX, angle_x)

def servo_centerY():
	
	global angle_y
	angle_y = 1500
	servo.set_servo(SERVOY, angle_y)

def fire():

	GPIO.output(FS, GPIO.HIGH)

def stop_fire():

	GPIO.output(FS, GPIO.LOW)

	
   
# -------------------------------------------------- #
# Initialization part                                #
# -------------------------------------------------- #

# Setup GPIOs
GPIO.setFunction(LS, GPIO.PWM)
GPIO.setFunction(L1, GPIO.OUT)
GPIO.setFunction(L2, GPIO.OUT)

GPIO.setFunction(FS, GPIO.OUT)
GPIO.setFunction(R1, GPIO.OUT)
GPIO.setFunction(R2, GPIO.OUT)

servo.set_servo(SERVOY, angle_y)
servo.set_servo(SERVOX, angle_x)
servo.set_servo(SERVOZ, angle_z)

set_speed(1)
stop()

# -------------------------------------------------- #
# Main server part                                   #
# -------------------------------------------------- #


# Instantiate the server on the port 8000, it starts immediately in its own thread
server = webiopi.Server(port=8000, login="webiopi", password="raspberry")

# Register the macros so you can call it with Javascript and/or REST API

server.addMacro(go_forward)
server.addMacro(go_backward)
server.addMacro(turn_left)
server.addMacro(turn_right)
server.addMacro(stop)
server.addMacro(servo_y_aum)
server.addMacro(servo_y_dim)
server.addMacro(servo_x_aum)
server.addMacro(servo_x_dim)
server.addMacro(servo_z_aum)
server.addMacro(servo_z_dim)
server.addMacro(servo_center)
server.addMacro(servo_centerX)
server.addMacro(servo_centerY)
server.addMacro(fire)
server.addMacro(stop_fire)
# -------------------------------------------------- #
# Loop execution part                                #
# -------------------------------------------------- #

# Run our loop until CTRL-C is pressed or SIGTERM received
webiopi.runLoop()

# -------------------------------------------------- #
# Termination part                                   #
# -------------------------------------------------- #

# Stop the server
server.stop()

# Reset GPIO functions
GPIO.setFunction(LS, GPIO.IN)
GPIO.setFunction(L1, GPIO.IN)
GPIO.setFunction(L2, GPIO.IN)

GPIO.setFunction(RS, GPIO.IN)
GPIO.setFunction(R1, GPIO.IN)
GPIO.setFunction(R2, GPIO.IN)

servo.stop_servo(SERVOY)
servo.stop_servo(SERVOX)
servo.stop_servo(SERVOZ)

#GPIO.setFunction(SERVOX, GPIO.IN)
#GPIO.setFunction(SERVOY, GPIO.IN)
#GPIO.setFunction(SERVOZ, GPIO.IN)