from __future__ import division
import RPi.GPIO as GPIO
from time import sleep
from serial import Serial
import json

import Adafruit_PCA9685

class Car(object):

    def __init__(self, BT_Port, laser_pin):
        GPIO.setmode(GPIO.BCM)
        self.ser = Serial(BT_Port, baudrate=9600,timeout=0)
        self.laser_pin = laser_pin
        self.laser_state = False
        GPIO.setup(self.laser_pin, GPIO.OUT)
        
    def configMotors(self, mL1, mL2, mR1, mR2, invertLR=False):
        self.motor_array = [mL1, mL2, mR1, mR2]
        self.invert = invertLR
        for pin in self.motor_array:
            GPIO.setup(pin, GPIO.OUT)
        #self.motor_pwm.append(GPIO.PWM(pin, 490))
        self.motorL1_pwm = GPIO.PWM(mL1, 100)
        self.motorL2_pwm = GPIO.PWM(mL2, 100)
        self.motorR1_pwm = GPIO.PWM(mR1, 100)
        self.motorR2_pwm = GPIO.PWM(mR2, 100)
        self.motorL1_pwm.start(0)
        self.motorL2_pwm.start(0)
        self.motorR1_pwm.start(0)
        self.motorR2_pwm.start(0)
        
    def configServos(self):
        self.pwm = Adafruit_PCA9685.PCA9685()
        self.servo_min = 150
        self.servo_max = 600
        self.pwm.set_pwm_freq(60)
        
    def deg_to_pulse(self, degree):
        pulse = (degree - 0) * (self.servo_max - self.servo_min) / (180 - 0) + 150 
        print('Pulse: '+str(pulse))
        return int(pulse)
        
    def receive_command(self):
        self.cmd = ""
        if  self.ser.inWaiting():
            self.cmd = self.ser.readline()
            if self.cmd != None:
                try:
                    data = self.cmd.decode('utf-8')
                    data = json.loads(data.strip())
                    print data
                    if data != "":
                        return data
                    else:
                        return ""
                except ValueError:
                    pass
        #except IOError:
            #pass
            
    def turn_wheel(self, motor1, motor2):
            if self.invert:
                temp = motor1
                motor1 = motor2
                motor2 = temp
            motor1 = (motor1 / 2)
            motor2 = (motor2 / 2)
            if motor1<0:
                self.motorL2_pwm.ChangeDutyCycle(motor1*-1)
                self.motorL1_pwm.ChangeDutyCycle(0)#make positive if negative
            else:
                self.motorL1_pwm.ChangeDutyCycle(motor1)
                self.motorL2_pwm.ChangeDutyCycle(0)
        
            if motor2<0:
                self.motorR2_pwm.ChangeDutyCycle(motor2*-1)
                self.motorR1_pwm.ChangeDutyCycle(0)#make positive if negative
            else:
                self.motorR1_pwm.ChangeDutyCycle(motor2)
                self.motorR2_pwm.ChangeDutyCycle(0)
        
    def move_turret(self, turret_deg, barrel_deg):
        self.pwm.set_pwm(0, 0, self.deg_to_pulse(turret_deg))
        self.pwm.set_pwm(1, 0, self.deg_to_pulse(barrel_deg))
            
    def fire_laser(self, state):
        GPIO.output(self.laser_pin, state)

    def update(self): #Run to make the robot work
        self.json_obj = self.receive_command()
        print self.json_obj
        try:
            if "motor" in self.cmd:
                if self.json_obj["motor"] != None:
                    val1 = self.json_obj["motor"][0]
            # if val1 > 0:
                 #    val1 += 10
                    val2 = self.json_obj["motor"][1]
            # if val2 <0:
            #   val2 += 4
                    self.turn_wheel(val1, val2)
            elif "shoot" in self.cmd:
                self.laser_state = self.json_obj["shoot"]
                self.fire_laser(self.json_obj["shoot"])
            elif "turret" in self.cmd:
                val1 = self.json_obj["turret"][0]
                val2 = self.json_obj["turret"][1]
                self.move_turret(val1, val2)
        
        except TypeError:
            pass
    sleep(.01)