# -*- coding: utf-8 -*-
#!/usr/bin/python
import pygame
from pygame.locals import *
from bluepy import btle
import math
import struct
import time
import BB8_driver
import sys
SCREEN_SIZE = (640, 480)  # 画面サイズ (横/縦)
STEP = 10       #キーボード/ハットスイッチの反応の良さ

bb8 = BB8_driver.Sphero()
bb8.connect()
pygame.init()
screen = pygame.display.set_mode(SCREEN_SIZE)   # SCREEN_SIZEの画面を作成
pygame.display.set_caption("window test")       # Windowタイトルの設定
X_CENTER = int(SCREEN_SIZE[0]/2)
Y_CENTER = int(SCREEN_SIZE[1]/2)
[circle_x, circle_y] = [X_CENTER, Y_CENTER]     #円の初期位置を設定

bb8.start()
time.sleep(2)
bb8.set_rgb_led(255,0,0,0,False)
time.sleep(1)
bb8.set_rgb_led(0,255,0,0,False)
time.sleep(1)
bb8.set_rgb_led(0,0,255,0,False)
time.sleep(3)

pygame.joystick.init()
joystick = pygame.joystick.Joystick(0) # create a joystick
joystick.init()

def sendRollCommand(speed, heading):
    lastHeading = 0
    if heading > 0:
       lastHeading = heading
    if speed > 0:
        bb8.roll(speed, heading, 1, False)
    else:
        bb8.roll(0, lastHeading, 0, False)

while True:
        screen.fill((0,0,0))     # 画面を青色で塗りつぶす
        # イベント処理
        for event in pygame.event.get():    #×ボタンによる終了
            if event.type == QUIT:  # 終了イベント
                bb8.join()
                bb8.disconnect()
                sys.exit()
        #  check left axis
        axis_x = joystick.get_axis(0)
        axis_y = -joystick.get_axis(1)  # axis_y direction is graphic one
        circle_x = int((axis_x+1) * X_CENTER)
        circle_y = int((axis_y+1) * Y_CENTER)
        joystickDistanceFromCenter = math.sqrt(axis_x * axis_x + axis_y * axis_y)
        lastHeading = 0
        pressed_keys = pygame.key.get_pressed()
        # 押されているキーに応じて画像を移動, clock wize means x,y exchange
        if joystickDistanceFromCenter > 0.3:
            joystickAngleDegrees = math.atan2(axis_x, axis_y)
            if axis_x < 0.0:
                joystickAngleDegrees += 2.0 * math.pi
            joystickAngleDegrees *= 180.0 / math.pi
            heading = int(joystickAngleDegrees)
            speed = int(joystickDistanceFromCenter*125)

            bb8.roll(speed, heading, 1, False)
            print "axis_x:", axis_x, " axis_y:", axis_y
            print "speed:", speed, "heading:", heading
            pygame.time.wait(1000)     # 1000msec 待ち
            bb8.roll(0, lastHeading, 1, False)
            circle_x = circle_x - STEP
            lastHeading = heading

        #描画範囲の上下限チェック
        if circle_x< 0:
            circle_x = 0
        elif circle_x > SCREEN_SIZE[0]:
            circle_x = SCREEN_SIZE[0]

        if circle_y< 0:
            circle_y = 0
        elif circle_y > SCREEN_SIZE[1]:
            circle_y = SCREEN_SIZE[1]

        #描画
        pygame.draw.circle(screen, (255,0,0), (circle_x,circle_y), 10)
        pygame.display.update()  # 画面を更新
        pygame.time.wait(30)     # 30msec 待ち