from pykinect2 import PyKinectV2
from pykinect2.PyKinectV2 import *
from pykinect2 import PyKinectRuntime
import math
import random

import ctypes
import _ctypes
import pygame
import sys
import inspect

if sys.hexversion >= 0x03000000:
    import _thread as thread
else:
    import thread


class BodyGameRuntime(object):
    def __init__(self):
        pygame.init()

        # Used to manage how fast the screen updates
        self._clock = pygame.time.Clock()
        self.count=0
        self.planting = False
        self.plants = []
        self.stem = pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/circle.png"), (20,20))
        self.sun = pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/sun.png"), (200,200))
        self.reset = pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/reset.png"), (200,200))
        self.seedImg = pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/seed.png"), (70,70))
        self.basket = pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/sunflowerseeds.png"), (300,300))
        self.bud = pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/flowerbud.png"), (70,70))
        self.plantHeads = []
        self.ground = pygame.image.load("/Users/jon/Downloads/soil.png")
        for i in range(16):
            self.plantHeads.append(pygame.transform.scale(pygame.image.load("/Users/jon/Downloads/flower"+str(i+1)+".png"), (100,100)))        # Set the width and height of the screen [width, height]
        self._infoObject = pygame.display.Info()
        self._screen = pygame.display.set_mode((self._infoObject.current_w >> 1, self._infoObject.current_h >> 1), 
                                               pygame.HWSURFACE|pygame.DOUBLEBUF|pygame.RESIZABLE, 32)

        pygame.display.set_caption("Kinect for Windows v2 Body Game")

        # Loop until the user clicks the close button.
        self._done = False

        # Used to manage how fast the screen updates
        self._clock = pygame.time.Clock()

        # Kinect runtime object, we want only color and body frames 
        self._kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Color | PyKinectV2.FrameSourceTypes_Body)

        # back buffer surface for getting Kinect color frames, 32bit color, width and height equal to the Kinect color frame size
        self._frame_surface = pygame.Surface((self._kinect.color_frame_desc.Width, self._kinect.color_frame_desc.Height), 0, 32)

        # here we will store skeleton data 
        self._bodies = None

    count = 0

    def update_objects(self, objs, xper, yper, speed):
        for i in range(len(objs)):
            x = objs[i][0]
            y = objs[i][1]
            dx = xper - x
            dy = yper - y
            dist = math.sqrt(dx*dx+dy*dy)
            dx /= dist
            dy /= dist
            dx *= speed
            dy *= speed
            objs[i] = (x+dx,y+dy)

        return objs
     
    def draw_color_frame(self, frame, target_surface):
        target_surface.lock()
        address = self._kinect.surface_as_array(target_surface.get_buffer())
        ctypes.memmove(address, frame.ctypes.data, frame.size)
        del address
        target_surface.unlock()

    def run(self):
        test = 0
        objects = []
        for i in range(10):
            objects.append((random.randint(0,self._frame_surface.get_width()), random.randint(0,self._frame_surface.get_height())))

        # -------- Main Program Loop -----------
        while not self._done:
            # --- Main event loop
            for event in pygame.event.get(): # User did something
                if event.type == pygame.QUIT: # If user clicked close
                    self._done = True # Flag that we are done so we exit this loop

                elif event.type == pygame.VIDEORESIZE: # window resized
                    self._screen = pygame.display.set_mode(event.dict['size'], 
                                               pygame.HWSURFACE|pygame.DOUBLEBUF|pygame.RESIZABLE, 32)
                    

            if self._kinect.has_new_color_frame():
                frame = self._kinect.get_last_color_frame()
                self.draw_color_frame(frame, self._frame_surface)
                frame = None

            if self._kinect.has_new_body_frame(): 
                self._bodies = self._kinect.get_last_body_frame()

            if self._bodies is not None: 
                for i in range(0, self._kinect.max_body_count):
                    body = self._bodies.bodies[i]
                    if body.is_tracked:  
              
                        joints = body.joints 
                        # convert joint coordinates to color space 
                        joint_points = self._kinect.body_joints_to_color_space(joints)
                        try:
                            leftX=int(joint_points[PyKinectV2.JointType_HandLeft].x)
                            leftY=int(joint_points[PyKinectV2.JointType_HandLeft].y)
                            rightX=int(joint_points[PyKinectV2.JointType_HandRight].x)
                            rightY=int(joint_points[PyKinectV2.JointType_HandRight].y)
                            xper = int(joint_points[PyKinectV2.JointType_SpineMid].x)
                            yper = int(joint_points[PyKinectV2.JointType_SpineMid].y)   
                        except:
                            leftX=0
                            leftY=0
                            rightX=0
                            rightY=0
                            xper = 0
                            yper = 0
                        objects = self.update_objects(objects, xper, yper, 5)

                        if (body.hand_right_state is not None):
                            rstate = body.hand_right_state

                        if (body.hand_left_state is not None):
                            lstate = body.hand_left_state

                        if (rstate == 3 and not self.planting  
                            and rightY < 400 and rightY > 0
                            and rightX > self._frame_surface.get_width() -400):
                            self.planting = True  

                        if (leftY < 400
                            and leftX < 400):
                            self.plants = []  

                        self._frame_surface.blit(self.reset,(0,0))

                        if self.planting:
                            self._frame_surface.blit(self.seedImg,(rightX-self.seedImg.get_width()//2,rightY-self.seedImg.get_height()//2))
                            if rightY > self._frame_surface.get_height() - 150:
                                self.plants.append((rightX,rightY,self.plantHeads[random.randint(0,len(self.plantHeads)-1)],[]))
                                self.planting = False
              
            else:
                xper = 0
                yper = 0
                self._frame_surface.blit(self.reset,(0,0))

            self._frame_surface.blit(self.sun,(xper-100,yper-100))

            self._frame_surface.blit(self.basket,(self._frame_surface.get_width()-300,0))

            self._frame_surface.blit(pygame.transform.scale(self.ground,(self._frame_surface.get_width(),150)),(0,self._frame_surface.get_height()-150))


            self.count += 1
            for i in range(len(self.plants)):
                x1 = self.plants[i][0]
                y1 = self.plants[i][1]
                p = self.plants[i][2]
                past = self.plants[i][3]
                if self.count == 30:
                    xdiff = x1 - xper
                    ydiff = y1 - yper
                    x1 -= xdiff / 150
                    y1 -= ydiff / 90
                    past.append((x1,y1))
                for j in range(len(past)-2):
                    past1 = past[j]
                    self._frame_surface.blit(self.stem, past1)
                if len(past) < 30:
                    self._frame_surface.blit(self.bud, (x1-30,y1-30))
                else:
                    self._frame_surface.blit(p, (x1-30,y1-30))
                self.plants[i] = (x1,y1,p,past)
            if self.count == 30:
                self.count = 0

            # --- copy back buffer surface pixels to the screen, resize it if needed and keep aspect ratio
            # --- (screen size may be different from Kinect's color frame size) 
            h_to_w = float(self._frame_surface.get_height()) / self._frame_surface.get_width()
            target_height = int(h_to_w * self._screen.get_width())
            surface_to_draw = pygame.transform.scale(self._frame_surface, (self._screen.get_width(), target_height));
            self._screen.blit(surface_to_draw, (0,0))
            surface_to_draw = None
            pygame.display.update()

            pygame.display.flip()

            self._clock.tick(60)

        self._kinect.close()
        pygame.quit()


__main__ = "Kinect v2 Body Game"
game = BodyGameRuntime();
game.run();