#!/usr/bin/env python2

#######################################################################
#                            Aufgabe 1                                #
#######################################################################

import pygame
import math
import argparse, sys

#import servo_ctrl, 
from iot_car import Iot_car

width = 400
height = 200

freq = 50  # Sets the frequency of input procession
delta = 1.0 / freq # time per step
frict = -1  # max friction

mouse_ctrl = False

v_step = True # Stufenweises regel der Beschlunigung in beide Richtungen
m_step = True

# vars for testmode
is_testmode_enabled = True
testmode_vars = {'is_servo_active': False}

# vars for car
motor = None
streeting = None


# start main pygame event processing loop here
pygame.display.init()

# set up the pygame screen enviroment
screen = pygame.display.set_mode((width, height))

# get a clock to generate frequent behaviour
clock = pygame.time.Clock()


# States of the keys
keystates = {
            'quit': False,
            'accelerate': False,
            'decelerate': False,
            'reset': False,
            'accelerate_angle_right': False,
            'accelerate_angle_left': False,
            'acceleration_mouse': False,
            'mouse_ctrl': False,   
            }

running = True

# Functions for exercises
    
def toggle_mouse_ctrl():
    global mouse_ctrl 
    mouse_ctrl= not mouse_ctrl
    
def mouse_ctrl_calc_velocity(pos):
    global height
    V_MAX = 11
    x, y = pos
    rel_y = y - (height/2)
    return -1 * (rel_y / (height/2.0)) * V_MAX
    
def mouse_ctrl_calc_angle(pos):
    global width
    ANGLE_MAX = 45
    x, y = pos
    rel_x = x - (width/2.0)
    return (rel_x / (width/2)) * ANGLE_MAX
    
if __name__ == "__main__":

    parser = argparse.ArgumentParser(description='IoT Lab script')
    parser.add_argument('--notest', action='store_true', default=False)
    args = parser.parse_args()
    if not args.notest:
        print "Running in testmode."
    
    car = Iot_car(testmode= not args.notest)
        
    try:
        while running:
            # set clock frequency
            clock.tick(freq);
            
            # process input events
            for event in pygame.event.get():
            
                # exit on quit
                if event.type == pygame.QUIT:
                    running = False

                # check for key down events (press)
                if event.type == pygame.KEYDOWN:
                    if event.key == pygame.K_q:
                        keystates['quit'] = True
                    elif event.key == pygame.K_w:
                        keystates['accelerate'] = True
                    elif event.key == pygame.K_s:
                        keystates['decelerate'] = True
                    elif event.key == pygame.K_d:
                        keystates['accelerate_angle_right'] = True
                    elif event.key == pygame.K_a:
                        keystates['accelerate_angle_left'] = True
                    elif event.key == pygame.K_m:
                        keystates['mouse_ctrl'] = True
                    elif event.key == pygame.K_r:
                        keystates['reset'] = True

                # check for key up events (release)
                if event.type == pygame.KEYUP:
                    if event.key == pygame.K_q:
                        keystates['quit'] = False
                    elif event.key == pygame.K_w:
                        v_step = True
                        car.is_testmode_servo_active = False
                        keystates['accelerate'] = False
                    elif event.key == pygame.K_s:
                        v_step = True
                        car.is_testmode_servo_active = False
                        keystates['decelerate'] = False
                    elif event.key == pygame.K_d:
                        keystates['accelerate_angle_right'] = False
                    elif event.key == pygame.K_a:
                        keystates['accelerate_angle_left'] = False
                    elif event.key == pygame.K_m:
                        keystates['mouse_ctrl'] = False
                        m_step = True
                    elif event.key == pygame.K_r:
                        v_step = True
                        keystates['reset'] = False
                        
                if event.type == pygame.MOUSEBUTTONDOWN:
                    # linke maustaste
                    if event.button == 1:
                        car.is_testmode_servo_active = True
                        keystates['acceleration_mouse'] = True
                        
                if event.type == pygame.MOUSEBUTTONUP:
                    if event.button == 1:
                        car.is_testmode_servo_active = False
                        keystates['acceleration_mouse'] = False
                        
                        
            # do something about the key states here, now that the event queue has been processed
            if keystates['quit']:
                running = False
            if keystates['reset']:
                # reset
                car.stop()
                mouse_ctrl = False
            if keystates['mouse_ctrl'] and m_step:
                #  beim umstellen zur maus steuerung und geschw. zuruecksetzen
                car.stop()
                toggle_mouse_ctrl()
                m_step = False
                print "Is mouse control enabled?: " + str(mouse_ctrl)
                
                
                
                
            # Calculate valeues for control types
            
            # MOUSE
            if mouse_ctrl:
                pos = pygame.mouse.get_pos()
                angle_cur = mouse_ctrl_calc_angle(pos)
                car.set_angle(mouse_ctrl_calc_angle(pos))
                if keystates['acceleration_mouse']:
                    speed_cur = mouse_ctrl_calc_velocity(pos)
                    car.accelerate(speed=speed_cur, direct=True)
                    
            # Keyboard
            else:
                if keystates['accelerate']: # and v_step:
                    # Beschleunigen
                    v_step = False
                    car.accelerate(speed=11)
                if keystates['decelerate']: # and v_step:
                    # abbremsen
                    car.accelerate(speed=-11)
                if keystates['accelerate_angle_right']:
                    car.streeting_right()
                if keystates['accelerate_angle_left']:
                    car.streeting_left()
                if not keystates['accelerate_angle_left'] and not keystates['accelerate_angle_right']:
                    car.reset_streeting()

            print("({},{} --> {})".format(car.speed_cur, car.angle_cur, (car.speed_cur - car.speed_last) / delta)) + ", Servo_active: " + str(car.is_testmode_servo_active)
            
            car.symulate()
        
    except KeyboardInterrupt:
        print "Exiting through keyboard event (CTRL + C)"
        
    # gracefully exit pygame here
    pygame.quit()