#!/usr/bin/env python3
import time
import logging
import atexit
from logdecorator import log_on_start, log_on_end, log_on_error

# this is necessary to fix the car when it powers down and restarts
try:
    from ezblock import *
    from ezblock import __reset_mcu__
    __reset_mcu__()
    time.sleep(0.01)
except ImportError:
    print("This  computer  does  not  appear  to be a PiCar -X system(/opt/ezblock  is not  present). Shadowing  hardware  calls with  substitute  functions")
    from sim_ezblock import *


logging_format = "%asctime)s: %(message)s"
logging.basicConfig(format=logging_format, level=logging.INFO, datefmt="%H:%M:%S")


PERIOD = 4095
PRESCALER = 10
TIMEOUT = 0.02


dir_servo_pin = Servo(PWM('P2'))
camera_servo_pin1 = Servo(PWM('P0'))
camera_servo_pin2 = Servo(PWM('P1'))
left_rear_pwm_pin = PWM("P13")
right_rear_pwm_pin = PWM("P12")
left_rear_dir_pin = Pin("D4")
right_rear_dir_pin = Pin("D5")

S0 = ADC('A0')
S1 = ADC('A1')
S2 = ADC('A2')

Servo_dir_flag = 1
dir_cal_value = 0
cam_cal_value_1 = 0
cam_cal_value_2 = 0
motor_direction_pins = [left_rear_dir_pin, right_rear_dir_pin]
motor_speed_pins = [left_rear_pwm_pin, right_rear_pwm_pin]
cali_dir_value = [1, -1]
cali_speed_value = [0, 0]
for pin in motor_speed_pins:
    pin.period(PERIOD)
    pin.prescaler(PRESCALER)

def set_motor_speed(motor, speed):
    global cali_speed_value,cali_dir_value
    motor -= 1
    if speed >= 0:
        direction = 1 * cali_dir_value[motor]
    elif speed < 0:
        direction = -1 * cali_dir_value[motor]
    speed = abs(speed)
    #if speed != 0:
    #    speed = int(speed /2 ) + 50
    speed = speed - cali_speed_value[motor]
    if direction < 0:
        motor_direction_pins[motor].high()
        motor_speed_pins[motor].pulse_width_percent(speed)
    else:
        motor_direction_pins[motor].low()
        motor_speed_pins[motor].pulse_width_percent(speed)

def motor_speed_calibration(value):
    global cali_speed_value,cali_dir_value
    cali_speed_value = value
    if value < 0:
        cali_speed_value[0] = 0
        cali_speed_value[1] = abs(cali_speed_value)
    else:
        cali_speed_value[0] = abs(cali_speed_value)
        cali_speed_value[1] = 0

def motor_direction_calibration(motor, value):
    # 0: positive direction
    # 1:negative direction
    global cali_dir_value
    motor -= 1
    if value == 1:
        cali_dir_value[motor] = -1*cali_dir_value[motor]


def dir_servo_angle_calibration(value):
    global dir_cal_value
    dir_cal_value = value
    set_dir_servo_angle(dir_cal_value)
    # dir_servo_pin.angle(dir_cal_value)

def set_dir_servo_angle(value):
    global dir_cal_value
    dir_servo_pin.angle(value+dir_cal_value)

def camera_servo1_angle_calibration(value):
    global cam_cal_value_1
    cam_cal_value_1 = value
    set_camera_servo1_angle(cam_cal_value_1)
    # camera_servo_pin1.angle(cam_cal_value)

def camera_servo2_angle_calibration(value):
    global cam_cal_value_2
    cam_cal_value_2 = value
    set_camera_servo2_angle(cam_cal_value_2)
    # camera_servo_pin2.angle(cam_cal_value)

def set_camera_servo1_angle(value):
    global cam_cal_value_1
    camera_servo_pin1.angle(-1 *(value+cam_cal_value_1))

def set_camera_servo2_angle(value):
    global cam_cal_value_2
    camera_servo_pin2.angle(-1 * (value+cam_cal_value_2))

def get_adc_value():
    adc_value_list = []
    adc_value_list.append(S0.read())
    adc_value_list.append(S1.read())
    adc_value_list.append(S2.read())
    return adc_value_list

def set_power(speed):
    set_motor_speed(1, speed)
    set_motor_speed(2, speed)

def backward(speed):
    set_motor_speed(1, -1*speed)
    set_motor_speed(2, speed)

def forward(speed):
    set_motor_speed(1, 1*speed)
    set_motor_speed(2, -1*speed)

def stop():
    set_motor_speed(1, 0)
    set_motor_speed(2, 0)


def Get_distance():
    timeout=0.01
    trig = Pin('D8')
    echo = Pin('D9')

    trig.low()
    time.sleep(0.01)
    trig.high()
    time.sleep(0.000015)
    trig.low()
    pulse_end = 0
    pulse_start = 0
    timeout_start = time.time()
    while echo.value()==0:
        pulse_start = time.time()
        if pulse_start - timeout_start > timeout:
            return -1
    while echo.value()==1:
        pulse_end = time.time()
        if pulse_end - timeout_start > timeout:
            return -2
    during = pulse_end - pulse_start
    cm = round(during * 340 / 2 * 100, 2)
    #print(cm)
    return cm

def stop_motors():
    set_motor_speed(1, 0)
    set_motor_speed(2, 0)
    camera_servo_pin1.angle(0)
    camera_servo_pin2.angle(0)
    set_dir_servo_angle(28)

def parallel_parking_right():
    set_dir_servo_angle(69)
    backward(50)
    time.sleep(1)
    stop()
    set_dir_servo_angle(0)
    backward(50)
    time.sleep(1)
    stop()
    set_dir_servo_angle(28)
    forward(35)
    time.sleep(0.1)
    stop()

def parallel_parking_left():
    set_dir_servo_angle(0)
    backward(50)
    time.sleep(0.5)
    stop()
    set_dir_servo_angle(69)
    backward(50)
    time.sleep(0.5)
    stop()
    set_dir_servo_angle(28)
    forward(35)
    time.sleep(1)

def user_control():
    while 1:
        print("---------------------------------")
        print("Available commands, Q to quit:\n")
        print('W or F for forward movement, S or B for backwards, D for parallel parking right, A for parallel parking left')
        print('J for a left 3 point turn, L for a right 3 point turn')
        command = input("Please choose a command: ")
        if command.upper() == "W" or command.upper() == "F":
            speed = int(input("\nPlease enter a speed to drive at (press enter for default of 50): ") or "50")
            angle = int(input("\nPlease enter an angle to drive at (press enter for default of straight): ") or "0")
            forward_angle(angle, speed)
        elif command.upper() == "S" or command.upper() == "B":
            speed = int(input("\nPlease enter a speed to drive at (press enter for default of 50): ") or "50")
            angle = int(input("\nPlease enter an angle to drive at (press enter for straight): " ) or "0")
            backward_angle(angle, speed)
        elif command.upper() == "D":
            parallel_parking_right()
        elif command.upper() == "A":
            parallel_parking_left()
        elif command.upper() == "J":
            kturn("left")
        elif command.upper() == "L":
            kturn("right")
        elif command.upper() == "Q":
            break
        else:
            print("Input was invalid. Try again")

def forward_angle(angle=0, speed=100):
    set_dir_servo_angle(angle+28)
    time.sleep(0.1)
    forward(speed)
    time.sleep(1)
    stop()

def backward_angle(angle=0, speed=100):
    set_dir_servo_angle(angle+28)
    time.sleep(0.1)
    backward(speed)
    time.sleep(1)
    stop()

def kturn(direction="RIGHT"):
    '''
    make a 3 point turn in a direction. 
    direction: starting turning left or right
    '''
    if direction.upper() == "LEFT":
        set_dir_servo_angle(-20)
        forward(50)
        time.sleep(1.25)
        set_dir_servo_angle(69)
        backward(50)
        time.sleep(1.75)
        set_dir_servo_angle(0)
        forward(50)
        time.sleep(1)
        stop()
        set_dir_servo_angle(28)
        forward(35)
        time.sleep(0.5)
        stop()
    else:
        set_dir_servo_angle(69)
        forward(50)
        time.sleep(1.25)
        set_dir_servo_angle(-20)
        backward(50)
        time.sleep(1.75)
        set_dir_servo_angle(0)
        forward(50)
        time.sleep(1)
        stop()
        set_dir_servo_angle(28)
        forward(35)
        time.sleep(0.5)
        stop()

def test(speed):
    # dir_servo_angle_calibration(-10)
    set_dir_servo_angle(28)
    #time.sleep(1)
    #set_dir_servo_angle(45)
    time.sleep(1)
    #set_dir_servo_angle(90)
    forward(speed)
    #set_motor_speed(1, -speed)
    #set_motor_speed(2, speed)
    camera_servo_pin1.angle(0)

atexit.register(stop_motors)

if __name__ == "__main__":
    user_control()
