#!/usr/bin/env python
#version    :       2024.03.11
#language   :       en
#hardware   :       PI

import threading
import time
import queue
import math
import os
import pyaudio
import wave
import sys
sys.path.append('/root/')
from CocoPi import QMI8658
from maix import camera,display,image

event=threading.Event()
showLock=threading.Lock()
event.set()
#配置字体
image.load_freetype("/root/preset/fonts/botPixel.ttf")

#导入图片资源对象
jumpbot_logo = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_128_128.png")
jumpbot_right_jump = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_right_jump.png")
jumpbot_left_jump = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_left_jump.png")
jumpbot_middle_jump = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_middle_jump.png")
jumpbot_middle_nojump = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_middle_nojump.png")
jumpbot_left_nojump = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_left_nojump.png")
jumpbot_right_nojump = image.open("/root/preset/img/jumpbot/oneHeadThreeLeg_right_nojump.png")



#比例转换函数
def numberMap(value, leftMin, leftMax, rightMin, rightMax):
    leftSpan = leftMax - leftMin
    rightSpan = rightMax - rightMin
    valueScaled = float(value - leftMin) / float(leftSpan)
    return int(rightMin + (valueScaled * rightSpan))

#音量调节函数
def voice_numberMap(value):
    valueScaled = float(value - 0) / float(100)
    return valueScaled * 31

#创建投影用的画布
img_drop=image.new(size=(320,240),mode="RGB")

#屏幕显示函数
def screenShow():
    from maix import image,display,camera
    
    import time
    global img_drop,lock4,lock5,queue1,queue2,screenCapture,x,y
    while True:
        event.wait()
        if x==1: 
            #queue1.put(1)
            try:
                #showLock.acquire()
                img_drop.draw_image((screenCapture.rotate(-90,adjust=1)),0,0)
            #print("rotate time："+str(time.time()))
                display.show(img_drop)
            finally:
                #showLock.release()
                pass
            x=0
            y=0
        else:
            pass
            #time.sleep(0.01)
    
#播放音乐

def playMusic():
    global lock3,jumpSound
    import wave
    import pyaudio
    import time
    p = pyaudio.PyAudio()
    while True:
        event.wait()
        if lock3==1:
            #导入音乐资源对象
            CHUNK = 1024
            jumpSound = wave.open(r"/root/preset/audio/jump1.wav", 'rb')
            stream = p.open(format=p.get_format_from_width(jumpSound.getsampwidth()),channels=jumpSound.getnchannels(),rate=jumpSound.getframerate(),output=True)
            data = jumpSound.readframes(CHUNK)
            while len(data) > 0:
                stream.write(data)
                data = jumpSound.readframes(CHUNK)
            stream.stop_stream()
            stream.close()
            #p.terminate()
            lock3=0
        else:
            time.sleep(0.01)

#获取MPU数值
def getMpuData():
    sys.path.append('/root/')
    from CocoPi import QMI8658
    import time
    global initData,lock1,lock2
    while True:
        if lock1==1:
            if event.is_set():
                event.clear()
            else:
                pass
            xyz=qmi8658.getPitchYawRollGxGyGz(initData)
            #print("xyz:"+str(xyz))
            pitch=round(xyz[0],2)
            yaw=round(xyz[1],2)
            #print("yaw:"+str(yaw))
            queue1.put(yaw)
            queue2.put(pitch)
            lock1=0
            lock2=1
            event.set()
        else:
            time.sleep(0.04)



#-------------------------------
#初始化色彩配置
splash_theme_color = (255,255,255)
splash_front_color = (60,60,60)
fontcolor=255
fontsize=0.75
fontchange=0

#开机显示
splash = image.new(size=(240, 320),mode = "RGB")
splash.draw_rectangle(0,0,240,320,color=splash_theme_color,thickness=-1)
splash.draw_string(40, 28, "Jump", 1.6, color=(255, 200, 0))
splash.draw_string(140, 28, "Cat", 1.6, color=(255, 128, 64))
splash.draw_image(jumpbot_logo.rotate(0, adjust=1),54,100,alpha=1)
splash.draw_string(40, 240, "NOTE:", 0.75, color=(0, 128, 255))
splash.draw_string(96, 240, "PLEASE", 0.75, color=(0, 128, 255))
splash.draw_string(160, 240, "PLACE", 0.75, color=(0, 128, 255))
splash.draw_string(40, 260, "HORIZONTALLY", 0.75, color=(0, 128, 255))
splash.draw_string(172, 260, "AND", 0.75, color=(0, 128, 255))
splash.draw_string(40, 280, "REMAIN", 0.75, color=(0, 128, 255))
splash.draw_string(110, 280, "STILL!", 0.75, color=(0, 128, 255))
screenCapture=splash.crop(0,0,240,320)
img_drop.draw_image((screenCapture.rotate(-90,adjust=1)),0,0)
display.show(img_drop)
time.sleep(1)

#配置QMI8658
qmi8658=QMI8658()
qmi8658.calibrate()
initData={}
initData["AcX"]=qmi8658.get_accel(10)["AcX"]
initData["AcY"]=qmi8658.get_accel(10)["AcY"]
initData["AcZ"]=qmi8658.get_accel(10)["AcZ"]
initData["GyX"]=qmi8658.get_accel(10)["GyX"]
initData["GyY"]=qmi8658.get_accel(10)["GyY"]
initData["GyZ"]=qmi8658.get_accel(10)["GyZ"]

#配置图形角色相关参数
x_speed=0
y_speed=0
x_pos=120
y_pos=160
x_change=0
y_change=5
fowardState=0
jumpState=0
Yaw=0

#配置音频
os.system("amixer cset numid=8,iface=MIXER,name='LINEOUT volume'voice_numberMap(25)")


#创建队列
queue1=queue.Queue()    #用来传递开启位置计算与陀螺仪数据获取的信号YAW
queue1.put(1)           #用来作为第一次开启位置计算与陀螺仪数据获取的信号
queue2=queue.Queue()    #用于存放图像数据

#创建信号锁
lock1=1
lock2=0
lock3=0
lock4=1
lock5=0

thread2=threading.Thread(target=getMpuData)
thread2.start()
thread3=threading.Thread(target=screenShow)
thread3.start()
#thread4=threading.Thread(target=playMusic)
#thread4.start()
x=0
y=0
while True:
    event.wait()
    if y==0:
        splash.clear()
        splash.draw_rectangle(0,0,240,320,color=splash_theme_color,thickness=-1)
        #splash.draw_string(80, 8, "pitch:"+str(round(xyz[0],2)), fontsize, color=(0, 0, 0))
        #splash.draw_string(80, 28, "yaw:"+str(round(xyz[1],2)), fontsize, color=(0, 0, 0))
        #splash.draw_string(80, 48, "fontcolor:"+str(fontcolor), fontsize, color=(0, 0, 0))
        #splash.draw_image(jumpbot_left_jump.rotate(0, adjust=1),109,149,alpha=1)
        #splash.draw_image(jumpbot_right_jump.rotate(0, adjust=0),109,149-int(xyz[0]),alpha=1)
        #splash.draw_image(jumpbot_right_jump.rotate(0, adjust=0),109-int(xyz[1]),149,alpha=1)

        #提示字显示
        splash.draw_rectangle(0,280,240,320,color=splash_front_color,thickness=-1)
        splash.draw_string(48, 286, "JUMP!", fontsize, color=(fontcolor,fontcolor,fontcolor))
        splash.draw_string(104, 286, "JUMP!", fontsize, color=(fontcolor,fontcolor,fontcolor))
        splash.draw_string(160, 286, "JUMP!", fontsize, color=(fontcolor,fontcolor,fontcolor))
        #splash.draw_string(48, 286, "PRESS", fontsize, color=(fontcolor,fontcolor,fontcolor))
        #splash.draw_string(104, 286, "B", fontsize, color=(fontcolor,fontcolor,fontcolor))
        #splash.draw_string(120, 286, "TO", fontsize, color=(fontcolor,fontcolor,fontcolor))
        #splash.draw_string(148, 286, "START", fontsize, color=(fontcolor,fontcolor,fontcolor))
        if fontcolor==splash_front_color[0]:
            fontchange=15
        elif fontcolor==255:
            fontchange=-15
        fontcolor=fontcolor+fontchange

        #获取Yaw值
        if lock2==1:
            Yaw=queue1.get()
            Pitch=queue2.get()
            lock2=0
            lock1=1
        else:
            pass

        if -90<=Yaw and Yaw<=0:
            fowardState=1
            x_speed=int(math.sqrt(math.fabs(Yaw)))
            #x_speed=numberMap(0-yaw,0,90,0,18)
        elif 0<Yaw and Yaw<=90:
            fowardState=0
            x_speed=-int(math.sqrt(math.fabs(Yaw)))
            #x_speed=numberMap(0-yaw,-90,0,-18,0)
        else:
            pass
        x_pos=x_pos+x_speed
        if x_pos<0:
            x_pos=240
        elif x_pos>240:
            x_pos=0
        else:
            pass
        if fowardState==0:
            sprite=jumpbot_left_nojump
        elif fowardState==1:
            sprite=jumpbot_right_nojump
        else:
            pass

        if y_pos+y_speed>259:
            y_pos=259
            y_speed=-35
            lock3=1
        else:
            y_pos=y_pos+y_speed
            y_speed=y_speed+y_change
        print("yaw:"+str(Yaw))
        splash.draw_image(sprite,x_pos-11,y_pos,alpha=1)
        splash.draw_string(136, 8, "SPEED:"+str(y_speed), fontsize, color=(0, 0, 0))
        splash.draw_string(144, 28, "YAW:"+str(Yaw), fontsize, color=(0, 128, 255))
        splash.draw_string(124, 48, "PITCH:"+str(Pitch), fontsize, color=(255, 128, 64))
        screenCapture=splash.crop(0,0,240,320)
        #queue2.put(1)
        x=1
        y=1