#!/usr/bin/env python3
#
# NOTE I HAVE MOVED ON TO USING A CUSTOM MARKER PRINTED OUT AND PASTED ON TOP OF MY CHARGER
# SIZED AT 4CM/SIDE COZMO HAS A MUCH BETTER CHANCE OF FINDING THE CHARGER - SIMILAR TO THE NEW VECTOR ROBOT
# the last "general purpose" version is https://gist.github.com/acidzebra/c02ff8c8ccb0e3a057ae0b48a5082a68/ad4ff9d686f7e2a1b197c4a26c6290d51a7c4380
# if you want to print out your own you can find a version of marker CustomObjectMarkers.Hexagons2 used here
# http://cozmosdk.anki.com/docs/generated/cozmo.objects.html?highlight=hexagons2#cozmo.objects.CustomObjectMarkers.Hexagons2
#
# WHAT THIS PROGRAM DOES
#
# SHORT VERSION: this program makes Cozmo play until he runs low on battery, then makes him find and dock with his charger. 
# Once his battery is fully recharged, he will leave the dock and start playing again. Rinse, repeat.
#
# You can run this program as-is but it is likely some things will need to be tweaked to suit your Cozmo and environment.
#
# AUTOMATIC DOCK FINDING AND DOCKING
# this is highly reliant on a number of factors, including having enough ambient light, how big the area Cozmo has to explore is,
# and just sheer blind luck sometimes. The built-in camera of Cozmo is not very high rez, and the dock icon is a little small.
# You can improve this in several ways - better lights, custom printed large battery icon (and some fiddling with the docking routing)
#
# The surface Cozmo moves on can be more slippery or less than my environment, this will impact things like docking.
# On my table's surface, I do two 95 degree turns, this makes it so Cozmo has a high rate of successful dockings.
# Look for these two lines in the code and adjust as necessary
# robot.turn_in_place(degrees(95)).wait_for_completed()
# robot.turn_in_place(degrees(95)).wait_for_completed()
#
# Your Cozmo's battery will have slightly different upper and lower limits.
# there is a lower threshold at which Cozmo will switch states and start looking for his charger.
# The variable is called lowbatvoltage, 3.7 is the setting that works for me. 
# a higher value will cause Cozmo to start looking for his dock sooner.
#
# have a look at the section called
# CONFIGURABLE VARIABLES
# for more stuff
#
# DETAILED VERSION:
# This program defines a number of 'states' for Cozmo:
# State 1: on charger, charging
# State 2: on charger, fully charged
# State 3: not on charger, battery starting to get low
# State 4: not on charger, good battery - freeplay active
# State 5: battery low, looking for charger
# State 6: battery low and we know where the charger is, moving to dock and docking
# State 9: Cozmo is on its side or is currently picked up
# State 0: recovery state - used to smooth state switching and to reassess which state Cozmo should be in
# state 99: "fake" state used when running a random animation
# state 98: "fake" state used when cozmo is looking around for his charger
#
# States dictate what Cozmo does. In state 4 Cozmo will enter freeplay mode, exploring and interacting with his environment: 
# stack cubes, build pyramids, offer people he sees fistbumps and games of peekaboo, and more. When his battery drops below 
# a certain threshold (set in the variable lowbatvoltage), he will start looking for his charger (state 5). 
# If he finds it he will attempt to dock  (state 6) and start charging (state 1). Once fully charged (state 2), he drives off
# the charger and the cycle repeats (state 4). State 3 is used as a 'delay' so a single drop in battery level won't
# immediately cause him to seek out his charger. States 9 and 0 handle being picked up, falling, being on his side, and eventual
# recovery.
#
# The battery level of Cozmo is also used to manipulate his three "needs" settings: repair, energy, play (the three metrics for
# 'happiness' that you see in the main screen of the Anki Cozmo app). As his battery level gets lower so will his overall mood 
# and innate reactions to events. (TLDR: he gets grumpier the lower his battery level is)
#
# An event monitor running in a seperate thread watches for events like being picked up, seeing a face, detecting a cliff, etc.
# Some events are just logged, others trigger different Cozmo states or actions.
#


#import required functions
import sys, os, datetime, random, time, math, re, threading
##import logging
import asyncio, cozmo, cozmo.objects, cozmo.util
from cozmo.util import degrees, distance_mm, speed_mmps, Pose
from cozmo.objects import CustomObject, CustomObjectMarkers, CustomObjectTypes

#external libraries used for camera annotation
#you will need to add these using pip/pip3
from PIL import ImageDraw, ImageFont
import numpy as np
#logging.basicConfig(filename='cozmo.log',level=logging.WARN)

# set up global variables
global robot
global freeplay
global tempfreeplay
global needslevel
global cozmostate
global scheduler_playokay
global msg
global start_time
global use_cubes
global charger
global maxbatvoltage
global highbatvoltage
global lowbatvoltage
global batlightcounter
global use_scheduler
global camera
global foundcharger
global lightstate
global robotvolume
global debugging
global batcounter
global chargermarker1

# initialize needed variables
freeplay = 0
lightstate = 0
batcounter = 0
robot = cozmo.robot.Robot
msg = 'No status'
q = None # dependency on queue variable for messaging instead of printing to event-content directly
thread_running = False # starting thread for custom events

#
#============================
# CONFIGURABLE VARIABLES HERE
#============================
#
# BATTERY THRESHOLDS
#
# low battery voltage - when voltage drops below this Cozmo will start looking for his charger
# high battery voltage - when cozmo comes off your charger fully charge, this value will self-calibrate
# maxbatvoltage - the maximum battery level as recorded when cozmo is on charger but no longer charging
# tweak this to suit your cozmo
lowbatvoltage = 3.7
highbatvoltage= 4.14
maxbatvoltage = 4.8
#
# CUBE USAGE
#
# whether or not to activate the cubes (saves battery if you don't)
# I almost always leave this off, he will still stack them and mess around with them
# some games like "block guard dog" will not come up unless the blocks are active
use_cubes = 1
#
# COZMO VOLUME 
# what volume Cozmo should play sounds at, value between 0 and 1
robotvolume = 0.2
#
# SCHEDULER USAGE
#
# whether or not to use the schedule to define allowed "play times"
# this code is a bit rough, use at your own risk
use_scheduler = 0
# 
# DEBUGGING
# when disabled, clears the screen status updates every cycle
debugging = 1

# END OF CONFIGURABLE VARIABLES
#


#
# CAMERA ANNOTATOR
#
#
@cozmo.annotate.annotator
def camera_info(image, scale, annotator=None, world=None, **kw):
	global camera
	d = ImageDraw.Draw(image)
	bounds = [3, 0, image.width, image.height]

	camera = world.robot.camera
	text_to_display = 'Exposure: %s ms\n' % camera.exposure_ms
	text_to_display += 'Gain: %.3f\n' % camera.gain
	text = cozmo.annotate.ImageText(text_to_display,position=cozmo.annotate.TOP_LEFT,line_spacing=2,color="white",outline_color="black", full_outline=True)
	text.render(d, bounds)

		
#
# MAIN PROGRAM LOOP START
#
def cozmo_unleashed(robot: cozmo.robot.Robot):
	cozmo.setup_basic_logging(general_log_level='WARN', protocol_log_level='WARN', protocol_log_messages='all', deprecated_filter='default')
	#os.system('cls' if os.name == 'nt' else 'clear')
	global cozmostate,chargermarker1,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, batlightcounter, lowbatvoltage, highbatvoltage, maxbatvoltage, use_scheduler,msg,objmsg,facemsg,camera, foundcharger, tempfreeplay
	#robot.world.charger = None
	#charger = None
	foundcharger = 0
	robot.set_robot_volume(robotvolume)
	if use_cubes == 1:
		robot.enable_freeplay_cube_lights(enable=True)
	#robot.enable_device_imu(enable_raw=False, enable_user=True, enable_gyro=True)
	# set up some camera stuff
	robot.world.image_annotator.add_annotator('camera_info', camera_info)
	camera = robot.camera
	camera.enable_auto_exposure()
	robot.enable_facial_expression_estimation(enable=True)
	robot.camera.image_stream_enabled = True
	robot.camera.color_image_enabled = False
	if use_cubes == 0:
		robot.world.disconnect_from_cubes()
	else:
		robot.world.connect_to_cubes()
	robot.enable_all_reaction_triggers(True)
	robot.enable_stop_on_cliff(True)
	q = None # dependency on queue variable for messaging instead of printing to event-content directly
	thread_running = False # starting thread for custom events
	robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1)
	needslevel = 1
	tempfreeplay = 0
	lowbatcount=0
	batlightcounter=0
	cozmostate = 0
	#some custom objects that I printed out and use as virtual walls, if you don't have them don't worry about it, it won't affect the program
	# wall_obj1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Circles2,  340, 120, 44, 44, True)
	# wall_obj2 = robot.world.define_custom_wall(CustomObjectTypes.CustomType02, CustomObjectMarkers.Circles4,  340, 120, 44, 44, True)
	# wall_obj3 = robot.world.define_custom_wall(CustomObjectTypes.CustomType03, CustomObjectMarkers.Circles5,  340, 120, 44, 44, True)
	# wall_obj4 = robot.world.define_custom_wall(CustomObjectTypes.CustomType04, CustomObjectMarkers.Triangles2,340, 120, 44, 44, True)
	# wall_obj5 = robot.world.define_custom_wall(CustomObjectTypes.CustomType05, CustomObjectMarkers.Triangles3,340, 120, 44, 44, True)
	# #wall_obj6 = robot.world.define_custom_wall(CustomObjectTypes.CustomType06, CustomObjectMarkers.Hexagons2, 120, 340, 44, 44, True)
	# wall_obj6 = robot.world.define_custom_wall(CustomObjectTypes.CustomType06, CustomObjectMarkers.Hexagons2, 44, 44, 44, 44, True)
	# wall_obj7 = robot.world.define_custom_wall(CustomObjectTypes.CustomType07, CustomObjectMarkers.Circles3,  120, 340, 44, 44, True)
	# wall_obj8 = robot.world.define_custom_wall(CustomObjectTypes.CustomType08, CustomObjectMarkers.Hexagons3, 120, 340, 44, 44, True)
	chargermarker1 = robot.world.define_custom_wall(CustomObjectTypes.CustomType01, CustomObjectMarkers.Hexagons2, 40, 40, 40, 40, True)
	# initialize event monitoring thread
	q = None
	monitor(robot, q)
	start_time = time.time()
	msg = 'initialization complete'
	robot_print_current_state('entering main loop')
# ENTERING STATE LOOP
	while True:
		#robot_backbackbatteryindicator()
		#robot_print_current_state('main loop checkpoint')
#

#State 1: on charger, charging
		if (robot.is_on_charger == 1) and (robot.is_charging == 1):
			if cozmostate != 1: # 1 is charging
				robot_print_current_state('switching to state 1')
				cozmostate = 1
				start_time = time.time()
				foundcharger = 0
				if robot.is_freeplay_mode_active:
					######robot.enable_all_reaction_triggers(False)
					robot.stop_freeplay_behaviors()
				freeplay = 0
				if use_cubes == 1:
					robot.world.disconnect_from_cubes()
			lowbatcount=0
			cozmostate = 1
			msg = 'state 1 checkpoint'
			robot_print_current_state('state 1 - charging')
			# once in a while make random snoring noises
			robot_check_sleep_snoring()
#
#State 2: on charger, fully charged
#
		if (robot.is_on_charger == 1) and (robot.is_charging == 0):
			cozmostate = 2
			robot_print_current_state('switching to state 2 - pausing 30 secs')
			time.sleep(30)
			if cozmostate != 2:  # 2 is fully charged
				maxbatvoltage = robot.battery_voltage
				robot_print_current_state('switching to state 2')
				cozmostate = 2
				lowbatcount=0
				foundcharger = 0
				if use_cubes == 1:
					robot.world.connect_to_cubes()
			msg = 'state 2 checkpoint'
			robot_print_current_state('state 2 - charged')
			robot.set_needs_levels(repair_value=1, energy_value=1, play_value=1)
			# robot.drive_off_charger_contacts().wait_for_completed()
			# robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
			robot_check_scheduler()
#
#State 3: not on charger, battery starting to get low
#
		# basic 'trigger guard' so Cozmo doesn't go to charger immediately if the voltage happens to dip below 3.7
		if (robot.battery_voltage <= lowbatvoltage) and (robot.is_on_charger == 0) and cozmostate != 5 and cozmostate != 6  and cozmostate !=98 and cozmostate != 1 and cozmostate != 2:
			lowbatcount += 1
			robot_set_needslevel()
			msg = 'state 3 checkpoint'
			robot_print_current_state('state 3 - low battery threshold breach %s' % str(lowbatcount))
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,1,False,False,False)
			# print("Event log      : %s" % str(msg))
			time.sleep(0.5)
		# if we dip below the threshold three times we switch to state 5
		if lowbatcount > 2 and (robot.is_on_charger == 0) and cozmostate !=5 and cozmostate !=6 and cozmostate !=99  and cozmostate !=98 and cozmostate != 1 and cozmostate != 2:
			if use_cubes == 1:
				robot.world.disconnect_from_cubes()
			robot_set_needslevel()
			msg = 'state 3 exit'
			robot_print_current_state('state 3 - low battery - switching to state 5')
			if cozmostate != 1 and cozmostate != 6:
				cozmostate = 5
#			
#State 4: not on charger, good battery - freeplay active
#
		if (robot.battery_voltage > lowbatvoltage) and (robot.is_on_charger == 0) and cozmostate != 9 and cozmostate != 5 and cozmostate != 6 and cozmostate != 3 and lowbatcount < 3 and cozmostate != 99  and cozmostate !=98:
			if cozmostate != 4: # 4 is freeplay
				msg = 'state 4 checkpoint'
				robot_print_current_state('freeplay - switching to state 4')
				cozmostate = 4
				if freeplay == 0:
					freeplay = 1
					start_time = time.time()
					try:
						robot.drive_wheels(40, 40, l_wheel_acc=50, r_wheel_acc=50, duration=1)
					except:
						robot_print_current_state('state 4 - failed to drive wheels')
					robot_reaction_chance(cozmo.anim.Triggers.OnSpeedtapGameCozmoWinHighIntensity,1,True,False,False)
					if use_cubes == 1:
						robot.world.connect_to_cubes()
					if not robot.is_freeplay_mode_active:
						#robot.enable_all_reaction_triggers(True)
						robot.start_freeplay_behaviors()
		if not robot.is_freeplay_mode_active and cozmostate == 4:
			robot_print_current_state('state 4 - re-enabling freeplay')
			freeplay = 1
			#robot.enable_all_reaction_triggers(True)
			robot.start_freeplay_behaviors()
			robot_set_needslevel()
		if cozmostate == 4:
			robot_check_randomreaction()
			time.sleep(0.5)

#
# state 5: battery low, looking for charger
#
		if cozmostate == 5 and tempfreeplay != 1:
			robot_print_current_state('switching to state 5')
			if robot.is_freeplay_mode_active:
				freeplay = 0
				robot.stop_freeplay_behaviors()
			#robot.enable_all_reaction_triggers(True)
			robot_locate_dock()
			# if cozmostate == 6:
				# pass
			# else:
				# cozmostate = 0

#
# state 6: battery low and we know where the charger is, moving to dock and docking
#
		if cozmostate == 6:
			robot_print_current_state('switching to state 6')
			if robot.is_freeplay_mode_active:
				#####robot.enable_all_reaction_triggers(False)
				robot.stop_freeplay_behaviors()
				freeplay = 0
				robot_print_current_state('state 6 - aborting freeplay')
				robot.abort_all_actions(log_abort_messages=True)
				robot.wait_for_all_actions_completed()
			freeplay = 0
			##robot_set_needslevel()
			robot_start_docking()
#
# state 9: we're on our side or are currently picked up
#
		if cozmostate == 9:
			robot_print_current_state('switching to state 9')
			robot_flash_backpacklights(4278190335)  # 4278190335 is red
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,100,False,False,False)
			while cozmostate == 9:
				robot_print_current_state('state 9 - anger loop')
				robot_set_needslevel()
				if not robot.is_falling and not robot.is_picked_up:
					robot_print_current_state('state 9 reset - switching to 0')
					cozmostate = 0
					lightstate = 0
					break
				if robot.is_freeplay_mode_active:
					#robot.enable_all_reaction_triggers(True)
					robot.stop_freeplay_behaviors()
					freeplay = 0
				robot.abort_all_actions(log_abort_messages=True)
				robot.clear_idle_animation()
				robot.wait_for_all_actions_completed()
				robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedLeft,100,False,False,False)
				robot_print_current_state('picked annoyed response 1')
				time.sleep(0.5)
				if not robot.is_falling and not robot.is_picked_up:
					robot_print_current_state('state reset - switching to 0')
					cozmostate = 0
					lightstate = 0
					break
				robot_reaction_chance(cozmo.anim.Triggers.TurtleRoll,100,False,False,False)
				robot_print_current_state('picked annoyed response 2')
				time.sleep(0.5)
				if not robot.is_falling and not robot.is_picked_up:
					robot_print_current_state('state reset - switching to 0')
					cozmostate = 0
					lightstate = 0
					break
				robot_reaction_chance(cozmo.anim.Triggers.CodeLabUnhappy,100,False,False,False)
				robot_print_current_state('picked annoyed response 3')
				time.sleep(0.5)
				robot_print_current_state('state 9 - loop complete')

#
# state 0: recovery state
#		
		if cozmostate == 0:
			robot_print_current_state('state 0')
			#robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,True,True)
			#robot.set_all_backpack_lights(cozmo.lights.white_light)
			lightstate = 0
			time.sleep(0.5)

#
# state 98: lookaround loop trying to find charger
#

#			
# state 99: animation is playing
#

#			
# we have looped through the states
#
		#robot_set_needslevel()
		#robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,True,True)
		#msg = 'state loop complete'
		#robot_check_randomreaction()
		#robot_print_current_state('cozmo_unleashed state program loop complete')
		time.sleep(0.5)
#
#
# END OF STATE LOOP
# 
	##robot_set_needslevel()
	robot_reaction_chance(cozmo.anim.Triggers.CodeLabTakaTaka,1,True,True,False)
	robot_print_current_state('exiting main loop - how did we get here?')

#
# ROBOT FUNCTIONS
#
def robot_set_backpacklights(color):
	global robot
	robot.set_backpack_lights_off()
	color1=cozmo.lights.Color(int_color=color, rgb=None, name=None)
	color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
	light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=2000, off_period_ms=1000, transition_on_period_ms=1500, transition_off_period_ms=500)
	light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=2000)
	light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=2000, transition_on_period_ms=500, transition_off_period_ms=1500)
	robot.set_backpack_lights(None, light1, light2, light3, None)

def robot_flash_backpacklights(color):
	global robot
	robot.set_backpack_lights_off()
	color1=cozmo.lights.Color(int_color=color, rgb=None, name=None)
	color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
	light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=250, transition_on_period_ms=375, transition_off_period_ms=125)
	light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=250, off_period_ms=250, transition_on_period_ms=250, transition_off_period_ms=500)
	light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=250, off_period_ms=500, transition_on_period_ms=125, transition_off_period_ms=375)
	robot.set_backpack_lights(None, light1, light2, light3, None)	

def robot_backbackbatteryindicator():
	global robot,highbatvoltage,lowbatvoltage,maxbatvoltage,lightstate,batlightcounter
	batmultiplier = ((highbatvoltage - lowbatvoltage)/3)+0.1
	chargebatmultiplier = ((maxbatvoltage - lowbatvoltage)/3)+0.1
	critbatmultiplier = ((lowbatvoltage - 3.5)/3)
	robotvoltage=(robot.battery_voltage)
	if not lightstate:
		lightstate = 0
	oldlightstate = lightstate
	if robotvoltage > (highbatvoltage-batmultiplier) and cozmostate==4:
		# bottom two lights on, third light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=1 and lightstate !=2:
			lightstate = 1
			color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1)
			light2=cozmo.lights.Light(on_color=color1)
			light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage > (highbatvoltage-(batmultiplier*1.5)) and robotvoltage <= (highbatvoltage-batmultiplier) and cozmostate==4:
		#bottom one light on, second light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=2 and lightstate !=3:
			lightstate = 2
			color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1)
			light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage > (highbatvoltage-(batmultiplier*2.5)) and robotvoltage <= (highbatvoltage-(batmultiplier*1.5)) and cozmostate==4:
		# # bottom one light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=3:
			lightstate = 3
			color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			light2=cozmo.lights.Light(on_color=color2)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage >= lowbatvoltage and cozmostate==4:
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=4:
			lightstate = 4
			color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500)
			light2=cozmo.lights.Light(on_color=color2)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	#robot_set_backpacklights(65535)  # 65535 is blue
	elif robotvoltage >= (maxbatvoltage-(chargebatmultiplier/2.5)) and robotvoltage <= (maxbatvoltage-(chargebatmultiplier/3.5)) and cozmostate==1:
		# # bottom one light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=7 and lightstate !=6 and lightstate !=5:
			lightstate = 7
			color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			light2=cozmo.lights.Light(on_color=color2)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage >= (maxbatvoltage-(chargebatmultiplier/1.0)) and robotvoltage <= (maxbatvoltage-chargebatmultiplier/2.5) and cozmostate==1:
		#bottom one light on, second light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=6 and lightstate !=5:
			lightstate = 6
			color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1)
			light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage >= maxbatvoltage and cozmostate==1:
		# bottom two lights on, third light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=5:
			lightstate = 5
			color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1)
			light2=cozmo.lights.Light(on_color=color1)
			light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	# elif robotvoltage >= maxbatvoltage and cozmostate==1:
		# batlightcounter +=1
		# if batlightcounter > 5 and lightstate !=8:
			# lightstate = 8
			# color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
			# color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			# light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500)
			# light2=cozmo.lights.Light(on_color=color2)
			# light3=cozmo.lights.Light(on_color=color2)
			# robot.set_backpack_lights(None, light3, light2, light1, None)
			# batlightcounter = 0
		# pass
	#robot_set_backpacklights(4278190335)  # 4278190335 is red

	elif robotvoltage >= (lowbatvoltage+(critbatmultiplier*1.5)) and robotvoltage <= (lowbatvoltage+critbatmultiplier) and cozmostate==5:
		#bottom one light on, second light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=10  and lightstate !=9:
			lightstate = 10
			color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1)
			light2=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage >= (lowbatvoltage+(critbatmultiplier*2.5)) and robotvoltage <= (lowbatvoltage+(critbatmultiplier*1.5)) and cozmostate==5:
		# # bottom one light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=11  and lightstate !=10:
			lightstate = 11
			color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			light2=cozmo.lights.Light(on_color=color2)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage >= (lowbatvoltage+critbatmultiplier) and cozmostate==5:
		# bottom two lights on, third light blinking
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=9:
			lightstate = 9
			color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1)
			light2=cozmo.lights.Light(on_color=color1)
			light3=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=1000, off_period_ms=1000, transition_on_period_ms=1000, transition_off_period_ms=1000)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	elif robotvoltage >= lowbatvoltage and cozmostate==5:
		batlightcounter +=1
		if batlightcounter > 5 and lightstate !=12:
			lightstate = 12
			color1=cozmo.lights.Color(int_color=4278190335, rgb=None, name=None)
			color2=cozmo.lights.Color(int_color=0, rgb=None, name=None)
			light1=cozmo.lights.Light(on_color=color1, off_color=color2, on_period_ms=500, off_period_ms=500, transition_on_period_ms=500, transition_off_period_ms=500)
			light2=cozmo.lights.Light(on_color=color2)
			light3=cozmo.lights.Light(on_color=color2)
			robot.set_backpack_lights(None, light3, light2, light1, None)
			batlightcounter = 0
		pass
	if lightstate==0 or lightstate==99:
		lightstate=99
		if robot.is_on_charger:
			robot_set_backpacklights(65535)  # 16711935 is green
		else:
			if robot.battery_voltage > lowbatvoltage:
				robot_set_backpacklights(16711935)  # 65535 is blue
			else :
				robot_set_backpacklights(4278190335)  # 4278190335 is red
		
	
def robot_set_needslevel():
	global robot, needslevel, msg
	needslevel = 1 - (4.05 - robot.battery_voltage)
	if needslevel < 0.1:
		needslevel = 0.1
	if needslevel > 1:
		needslevel = 1
	# i = random.randint(1, 1000)
	# if i >= 990:
	#robot_print_current_state('updating needs levels')
	robot.set_needs_levels(repair_value=needslevel, energy_value=needslevel, play_value=needslevel)

def robot_check_sleep_snoring():
	global robot
	i = random.randint(1, 1000)
	if i >= 997 and cozmostate == 1 and not robot.is_animating:
		robot_print_current_state('check complete - snore')
		#robot.play_anim_trigger(Sleeping).wait_for_completed()
		robot_reaction_chance(cozmo.anim.Triggers.Sleeping,100,True,False,True)
	else:
		#robot_print_current_state('check complete - no snore')
		time.sleep(0.5)

def robot_check_scheduler():
	global robot,scheduler_playokay,use_cubes,use_scheduler, highbatvoltage
	# from 7pm on weekdays
	weekdaystartplay = 19
	# to 11pm on weekdays
	weekdaystopplay  = 23
	# from 7am on weekends
	weekendstartplay = 7
	# to 11pm on weekends
	weekendstopplay  = 23
	# scheduler - when battery is charged this represents the chance cozmo will get off his charger to play
	# chance is defined as a number between 1-99 with a higher number representing a lesser chance
	playchance = 1
	robot_print_current_state('starting schedule check')
	# day and time check - are we okay to play at this time and day?
	day_of_week = datetime.date.today().weekday() # 0 is Monday, 6 is Sunday
	ctime = datetime.datetime.now().time()
	scheduler_playokay=0
	#it's weekend! Check for allowed times.
	if day_of_week > 4:
		if (ctime > datetime.time(weekendstartplay) and ctime < datetime.time(weekendstopplay)):
			scheduler_playokay=1
	#it's a weekday! Check for allowed times.
	else:
		if (ctime > datetime.time(weekdaystartplay) and ctime < datetime.time(weekdaystopplay)):
			scheduler_playokay=1
	# are we using the scheduler?
	if use_scheduler==0:
		scheduler_playokay=1
	# if the schedule says OK roll dice to see if we wake up
	if scheduler_playokay==1:
		robot_print_current_state('schedule OK - random chance to wake up')
		i = random.randint(1, 100)
		# wake up chance
		if use_scheduler==0:
			i = 100
		if i >= playchance:
			robot_print_current_state('waking up - leaving charger')
			#robot.world.connect_to_cubes()
			#robot_set_backpacklights(16711935)  # 16711935 is green
			try:
				robot.play_anim("anim_gotosleep_getout_02").wait_for_completed()
			except:
				robot_print_current_state('wake up anim failed')
				#pass
			for _ in range(3):
				try:
					robot.drive_off_charger_contacts().wait_for_completed()
					robot_print_current_state('drive off charger OK')
				except: 
					robot_print_current_state('drive off charger error')
					#pass
			time.sleep(0.5)
			highbatvoltage = robot.battery_voltage
			try:
				robot.move_lift(-3)
			except:
				robot_print_current_state('lift reset fail')
				#pass
			try:
				robot.drive_straight(distance_mm(60), speed_mmps(50)).wait_for_completed()
			except:
				robot_print_current_state('drive straight fail')
				#pass
			try:
				robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
			except:
				robot_print_current_state('drive straight fail')
				#pass
	# we're out of schedule or didn't make the dice roll, back off and check again later.
	x = 1
	while x < 20 and (robot.is_on_charger == 1):
		if scheduler_playokay == 1:
			robot_print_current_state('battery charged - schedule ok - not active')
			time.sleep(1)
		else:
			#print("State:  charged,  not active by schedule, sleep loop %d of 30 before next check." % (x))
			robot_print_current_state('battery charged - out of schedule')
			time.sleep(1)
			robot_check_sleep_snoring()
		if (robot.is_on_charger == 0):
			robot_print_current_state('cozmo was removed from charger')
			break
		time.sleep(2)

def robot_check_randomreaction():
	global robot,cozmostate,freeplay
	i = random.randint(1, 1000)
	#if i >= 980 and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and not robot.is_behavior_running and cozmostate==4:
	if i >= 970 and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and cozmostate==4:
		oldcozmostate=cozmostate
		cozmostate=99
		#random action!
		robot_print_current_state('random animation starting')
		if robot.is_freeplay_mode_active:
			#robot.enable_all_reaction_triggers(True)
			robot.stop_freeplay_behaviors()
		robot.abort_all_actions(log_abort_messages=True)
		#robot.clear_idle_animation()
		#robot.wait_for_all_actions_completed()
		#robot.wait_for_all_actions_completed()
		# grab a list of animation triggers
		all_animation_triggers = robot.anim_triggers
		# randomly shuffle the animations
		random.shuffle(all_animation_triggers)
		# select the first animation from the shuffled list
		triggers = 1
		chosen_triggers = all_animation_triggers[:triggers]
		print('Playing {} random animations:'.format(triggers))
		for trigger in chosen_triggers:
			if 'Onboarding' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'MeetCozmo' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'list' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'List' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Severe' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'TakaTaka' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Test' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Loop' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Sleep' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Request' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Singing' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'Drone' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			if 'SoundOnly' in trigger:
				trigger = 'cozmo.anim.Triggers.SparkSuccess'
			print("trigger %s" %str(trigger)," executed")
			robot_print_current_state('playing random animation')
			try:
				robot.play_anim_trigger(trigger).wait_for_completed()
			except:
				robot_print_current_state('random animation play failed')
				#pass
			robot_print_current_state('played random animation')
		#robot.wait_for_all_actions_completed()	
		if freeplay == 1 and not robot.is_freeplay_mode_active:
			#robot.enable_all_reaction_triggers(True)
			robot.start_freeplay_behaviors()
		if cozmostate==6:
			cozmostate=6
		else:
			cozmostate=oldcozmostate
		time.sleep(0.5)

def robot_reaction_chance(animation,chance,ignorebody,ignorehead,ignorelift):
	global robot, msg, freeplay,cozmostate
	i = random.randint(1, 100)
	if i >= chance and not robot.is_carrying_block and not robot.is_picking_or_placing and not robot.is_pathing and cozmostate !=99:
		robot_print_current_state('starting animation')
		oldcozmostate=cozmostate
		cozmostate=99
		oldfreeplay = 0
		if freeplay == 1:
			if robot.is_freeplay_mode_active:
				robot_print_current_state('disabling freeplay')
				#robot.enable_all_reaction_triggers(True)
			oldfreeplay = 1
			freeplay = 0
		robot.stop_freeplay_behaviors()
		robot.abort_all_actions(log_abort_messages=True)
		robot.stop_all_motors()
		robot_print_current_state('action queue aborted')
		#robot.clear_idle_animation()
		robot.wait_for_all_actions_completed()
		try:
			robot_print_current_state('playing animation')
			robot.play_anim_trigger(animation, ignore_body_track=ignorebody, ignore_head_track=ignorehead, ignore_lift_track=ignorelift).wait_for_completed()
			#print("reaction %s" %str(animation)," executed")
			msg = ("reaction %s" %str(animation)," executed")
			robot_print_current_state('animation completed')
		except:
			robot_print_current_state('animation failed')
			#print("reaction %s" %str(animation)," aborted")
			
		robot.wait_for_all_actions_completed()
		try:
			robot_print_current_state('resetting head angle')
			robot.set_head_angle(degrees(0)).wait_for_completed()
		except:
			robot_print_current_state('head angle reset failed')
			#print("reaction %s" %str(animation)," aborted")
		#robot.wait_for_all_actions_completed()
		robot.wait_for_all_actions_completed()
		try:
			robot_print_current_state('resetting lift')
			robot.move_lift(-3)
		except:
			robot_print_current_state('lift move down failed')
			#print("reaction %s" %str(animation)," aborted")
		robot.wait_for_all_actions_completed()
		if oldfreeplay == 1:
			oldfreeplay = 0
			freeplay = 1
			robot_print_current_state('re-enabling freeplay')
			if not robot.is_freeplay_mode_active:
				#robot.enable_all_reaction_triggers(True)
				robot.start_freeplay_behaviors()
		if cozmostate == 6:
			cozmostate =6
		elif cozmostate == 1:
			cozmostate = 1
		else:
			cozmostate=oldcozmostate
	else:
		time.sleep(0.5)
		robot_print_current_state('animation check - no winner')

def robot_locate_dock():
	global robot,cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, tempfreeplay
	#back off from whatever we were doing
	#robot_set_backpacklights(4278190335)  # 4278190335 is red
	if robot.is_freeplay_mode_active:
		robot_print_current_state('disabling freeplay')
		robot.stop_freeplay_behaviors()
	#robot.enable_all_reaction_triggers(True)
	robot.abort_all_actions(log_abort_messages=True)
	robot_print_current_state('starting locate dock sequence')
	robot.clear_idle_animation()
	#robot.wait_for_all_actions_completed()
	if use_cubes==1:
		robot.world.disconnect_from_cubes()
	freeplay = 0
	robot_reaction_chance(cozmo.anim.Triggers.NeedsMildLowEnergyRequest,1,False,False,False)
	try:
		robot.drive_straight(distance_mm(-30), speed_mmps(50)).wait_for_completed()
	except:
		robot_print_current_state('drive straight failed')
	robot_set_needslevel()
	robot_print_current_state('finding charger')
	# charger location search
	if robot.world.charger and cozmostate != 1 and cozmostate != 2 and cozmostate !=6:
		if robot.world.charger.pose.is_comparable(robot.pose):
			charger = robot.world.charger
			#we know where the charger is
			robot_print_current_state('finding charger, charger position known')
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,False)
			time.sleep(0.5)
			cozmostate = 6
			foundcharger = 1
		else:
			robot_print_current_state('finding charger, charger not in expected location')
			charger = None
			robot.world.charger = None
			if cozmostate != 1 and cozmostate != 2:
				cozmostate = 5
	charger = robot.world.charger
	if not charger and cozmostate != 1 and cozmostate != 2 and cozmostate !=6:
		robot_print_current_state('looking for charger')
		cozmostate = 5
		robot_reaction_chance(cozmo.anim.Triggers.SparkIdle,30,True,False,True)
		try:
			robot.move_lift(-3)
		except:
			robot_print_current_state('failed to move lift')
		try:
			robot.set_head_angle(degrees(0)).wait_for_completed()
		except:
			robot_print_current_state('failed to set head angle')
		try:
			robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive')
		# randomly drive around for a bit and see if we can spot the charger
		robot_drive_random_pattern()
		robot_print_current_state('looking for charger, random drive loop complete')
	time.sleep(0.5)

						
def robot_drive_random_pattern():
	global cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger, chargermarker1
	loops=5
	while loops>0 and cozmostate == 5:
		if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
			loops=0
			charger = robot.world.charger
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True)
			robot_print_current_state('found charger, breaking loop')
			cozmostate = 6
			foundcharger = 1
			break
		if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
			robot_print_current_state('breaking out of drive loop')
			loops=0
			break
		# drive to a random point and orientation
		counter=0
		while counter < 1 and cozmostate ==5 and cozmostate !=6:
			if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
				robot_print_current_state('breaking out of drive loop')
				loops=0
				break
			if random.choice((True, False)):
				x=150
			else:
				x=-150
			if random.choice((True, False)):
				y=150
			else:
				y=-150
			z= random.randrange(-40, 41, 80)
			robot_print_current_state('looking for charger, going to random pose')
			try:
				robot.go_to_pose(Pose(x, y, 0, angle_z=degrees(z)), relative_to_robot=True).wait_for_completed()
				robot.set_head_light(False)
				time.sleep(0.25)
				robot.set_head_light(True)
				time.sleep(0.25)
				robot.set_head_light(False)
			except:
				robot_print_current_state('failed to go to pose')
			if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
				loops=0
				break
			if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
				loops=0
				charger = robot.world.charger
				cozmostate = 6
				foundcharger = 1
				robot_print_current_state('found charger, breaking')
				robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True)
				break
			# else:
			robot_check_randomreaction()
			counter+=1
		if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
			loops=0
			break
		robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,False,True)
		# turn around for a bit
		time.sleep(0.5)
		counter=0
		
		oldcozmostate = cozmostate
		cozmostate = 98
		robot_print_current_state('start lookaround behavior')
		robot.set_head_angle(degrees(20)).wait_for_completed()
		# if chargermarker1.pose.is_comparable(robot.pose):
			# action = robot.go_to_pose(pose=chargermarker1.pose)
			# action.wait_for_completed()
		# else:
		look_around = robot.start_behavior(cozmo.behavior.BehaviorTypes.LookAroundInPlace)
		try:
			custom_objects = robot.world.wait_until_observe_num_objects(num=1, object_type = CustomObject, timeout=15)
			robot_print_current_state('custom object found')
		except asyncio.TimeoutError:
			robot_print_current_state('lookaround search timeout')
		finally:
			robot_print_current_state('stop lookaround')
			look_around.stop()
		if len(custom_objects) > 0:
			robot_print_current_state('custom object in array')
			found_object = custom_objects[0]
			if str(found_object.object_type) == "CustomObjectTypes.CustomType01":
				robot_print_current_state('custom object found, traveling')
				action = robot.go_to_pose(pose=found_object.pose)
				action.wait_for_completed()
				robot.set_head_angle(degrees(20)).wait_for_completed()
				robot.drive_straight(distance_mm(-80), speed_mmps(50)).wait_for_completed()
				robot.drive_straight(distance_mm(-80), speed_mmps(50)).wait_for_completed()
			else:
				#print("no object to drive to")
				robot_print_current_state('custom object not in array')
					#pass
		
				# try:
			# charger = robot.world.wait_for_observed_charger(timeout=8)
			# #print("Found charger: %s" % charger)
			# robot_print_current_state('found charger, breaking')
			# cozmostate=6
		# except asyncio.TimeoutError:
			# #print("Didn't see the charger")
			# robot_print_current_state('charger not found')
			# cozmostate = 5
		# finally:
			# # whether we find it or not, we want to stop the behavior
			# #robot_print_current_state('lookaround behavior ending')
			# look_around.stop()
		#cozmostate = oldcozmostate
		if cozmostate == 6 or cozmostate ==1 or cozmostate == 2:
			break
		robot_reaction_chance(cozmo.anim.Triggers.CodeLabChatty,1,True,False,True)
		robot_print_current_state('ended lookaround behavior')
		# while counter <2 and cozmostate == 5:
			# a= random.randrange(8, 17, 8)
			# t= random.randrange(2, 4, 1)
			# if random.choice((True, False)):
				# rx=50
			# else:
				# rx=-50
			# ry=-rx
			# robot_print_current_state('looking for charger, rotating')
			# try:
				# robot.set_head_light(False)
				# time.sleep(0.2)
				# robot.set_head_light(True)
				# time.sleep(0.2)
				# robot.set_head_light(False)
				# robot.drive_wheels(rx, ry, l_wheel_acc=a, r_wheel_acc=a, duration=t)
				# time.sleep(0.5)
			# except:
				# pass
		# if cozmostate == 6:
			# break
		if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
			loops=0
			charger = robot.world.charger
			robot_print_current_state('found charger')
			foundcharger = 1
			robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,True,False,True)
			break
		# else:
		robot_check_randomreaction()
			# counter+=1
		
		# if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
			# loops=0
			# charger = robot.world.charger
			# cozmostate = 6
			# foundcharger = 1
			# robot_print_current_state('found charger')
			# robot_reaction_chance(cozmo.anim.Triggers.CodeLabSurprise,1,False,False,False)
			# break
		#robot_set_needslevel()
		robot_print_current_state('looking for charger, looping through random poses')
		loops=loops-1
		if cozmostate == 6:
			cozmostate = 6
		else:
			cozmostate = oldcozmostate
	if cozmostate == 6:
		cozmostate = 6
	elif cozmostate == 1:
		cozmostate = 1
	elif cozmostate == 2:
		cozmostate = 2
	else:
		cozmostate = oldcozmostate
	robot_print_current_state('looking for charger, broke out of drive loop')
	#return charger

def robot_start_docking():
	global robot,cozmostate,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger
	charger = robot.world.charger
	#action = robot.go_to_object(charger, distance_mm(65.0))
	#action.wait_for_completed()
	robot_print_current_state('go to object complete')
	if robot.world.charger and robot.world.charger.pose.is_comparable(robot.pose):
		action = robot.go_to_pose(robot.world.charger.pose)
		action.wait_for_completed()
		robot_print_current_state('go to pose complete')
		robot.drive_straight(distance_mm(-50), speed_mmps(50)).wait_for_completed()
		robot_print_current_state('drove back a little bit')
	else: 
		robot_print_current_state('charger pose not known')
		robot.world.charger = None
		charger = None
		if cozmostate != 1 and cozmostate != 2:
			cozmostate = 5
		try:
			robot.play_anim_trigger(cozmo.anim.Triggers.ReactToPokeReaction, ignore_body_track=True, ignore_head_track=True, ignore_lift_track=True).wait_for_completed()
		except:
			robot_print_current_state('failed to play anim')
		robot_print_current_state('charger not found, clearing map')
		if cozmostate != 1 and cozmostate != 2:
			cozmostate = 5
	dockloop = 0
	while dockloop < 2 and cozmostate == 6 and robot.world.charger:
		try:
			action = robot.go_to_pose(robot.world.charger.pose)
			action.wait_for_completed()
		except:
			robot_print_current_state('failed to go to pose')
		robot_print_current_state('I should be in front of the charger')
		robot.world.charger = None
		robot.set_head_light(False)
		time.sleep(0.5)
		robot.set_head_light(True)
		time.sleep(0.5)
		robot.set_head_light(False)
		try:
			robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive')
		robot.set_head_light(False)
		time.sleep(0.5)
		robot.set_head_light(True)
		time.sleep(0.5)
		robot.set_head_light(False)
		try:
			action = robot.go_to_pose(robot.world.charger.pose)
			action.wait_for_completed()
		except:
			robot_print_current_state('failed to go to pose')
		try:
			robot.drive_straight(distance_mm(-20), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to back up a little bit')
		charger = robot.world.charger
		robot_reaction_chance(cozmo.anim.Triggers.FeedingReactToShake_Normal,85,True,False,False)
		robot_print_current_state('docking')
		try:
			robot.turn_in_place(degrees(95)).wait_for_completed()
			robot.turn_in_place(degrees(95)).wait_for_completed()
		except:
			robot_print_current_state('failed to do a 180')
		time.sleep(0.5)
		robot_reaction_chance(cozmo.anim.Triggers.CubePounceFake,1,True,False,False)
		try:
			robot.drive_straight(distance_mm(-147), speed_mmps(150)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive onto charger')
		time.sleep(0.5)
		# check if we're now docked
		if robot.is_on_charger:
			robot_reaction_chance(cozmo.anim.Triggers.SparkSuccess,1,True,False,True)
			dockloop = 3
			cozmostate = 1
			break
		# No, we missed. Back off and try again
		robot_print_current_state('failed to dock')
		robot_reaction_chance(cozmo.anim.Triggers.AskToBeRightedRight,1,True,False,False)
		try:
			robot.move_lift(-3)
		except:
			robot_print_current_state('failed to move lift')
		try:
			robot.set_head_angle(degrees(0)).wait_for_completed()
		except:
			robot_print_current_state('failed to set head angle')
		#os.system('cls' if os.name == 'nt' else 'clear')
		robot_print_current_state('failed to dock, retrying')
		try:
			robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive')
		try:
			robot.turn_in_place(degrees(-3)).wait_for_completed()
		except:
			robot_print_current_state('failed to turn')
		try:
			robot.drive_straight(distance_mm(100), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive')
		try:
			robot.turn_in_place(degrees(94)).wait_for_completed()
		except:
			robot_print_current_state('failed to turn')
		try:
			robot.turn_in_place(degrees(94)).wait_for_completed()
		except:
			robot_print_current_state('failed to turn')
		try:
			robot.set_head_angle(degrees(0)).wait_for_completed()
		except:
			robot_print_current_state('failed to set head angle')
		time.sleep(0.5)
		dockloop+=1
	# exited loop, check current state:
	if cozmostate != 1 and cozmostate != 2:
		cozmostate = 5
		charger= None
		robot.world.charger=None
		# express frustration
		try:
			robot.drive_straight(distance_mm(50), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive')
		try:
			robot.turn_in_place(degrees(-3)).wait_for_completed()
		except:
			robot_print_current_state('failed to turn')
		try:
			robot.drive_straight(distance_mm(80), speed_mmps(50)).wait_for_completed()
		except:
			robot_print_current_state('failed to drive')
		robot_drive_random_pattern()
		robot_reaction_chance(cozmo.anim.Triggers.MemoryMatchPlayerWinGame,1,True,False,False)
		x=0
		while x<11 and cozmostate == 5:
			tempfreeplay = 1
			if freeplay==0:
				freeplay = 1
				robot_print_current_state('charger not found, falling back to freeplay')
				#robot_set_backpacklights(16711935) # green
				if use_cubes==1:
					robot.world.connect_to_cubes()
				if not robot.is_freeplay_mode_active:
					robot_print_current_state('freeplay enabled')
					#robot.enable_all_reaction_triggers(True)
					robot.start_freeplay_behaviors()
			if cozmostate != 5:
				break
			robot_print_current_state('charger not found, falling back to freeplay')
			time.sleep(1)
		
			if robot.world.charger:
				robot_print_current_state('found charger while in temporary freeplay')
				charger = robot.world.charger
				cozmostate = 6
				break
			
			time.sleep(5)
			x+=1
		#after time expires or spotting the charger end freeplay
		tempfreeplay = 0
		if robot.is_freeplay_mode_active:
			#robot.enable_all_reaction_triggers(True)
			robot.stop_freeplay_behaviors()
		if use_cubes==1:
			robot.world.disconnect_from_cubes()
		#robot_set_backpacklights(4278190335) # red
		freeplay = 0
		if cozmostate != 1:
			cozmostate = 5
		#os.system('cls' if os.name == 'nt' else 'clear')
		robot_print_current_state('temporary freeplay ended')
		time.sleep(1)
#
# END OF ROBOT FUNCTIONS
#

	
#
# EVENT MONITOR FUNCTIONS
#
class CheckState (threading.Thread):
	global robot,cozmostate,freeplay,msg,camera,objmsg,facemsg
	def __init__(self, thread_id, name, _q):
		threading.Thread.__init__(self)
		self.threadID = thread_id
		self.name = name
		self.q = _q

# main thread
	def run(self):
		global robot,cozmostate,freeplay,msg,camera,highbatvoltage,lowbatvoltage,lightstate
		delay = 10
		is_picked_up = False
		is_falling = False
		is_on_charger = False
		is_cliff_detected = False
		is_moving = False
		is_carrying_block = False
		is_localized = False
		is_picking_or_placing = False
		is_pathing = False
		is_behavior_running = False
		while thread_running:
			
# event monitor: robot is picked up detection

			#robot_backbackbatteryindicator()

			if robot.is_picked_up:
				delay = 0
				if not is_picked_up:
					is_picked_up = True
					robot_flash_backpacklights(4278190335)  # 4278190335 is red
					cozmostate = 9
					robot_print_current_state('switching to state 9 - picked up')
					lightstate=0
			elif is_picked_up and delay > 9:
				cozmostate = 0
				lightstate=0
				is_picked_up = False
				robot_print_current_state('no longer picked up - state 0')
			elif delay <= 9:
				delay += 1
				
# event monitor: robot is carrying a block

			if robot.is_carrying_block:
				if not is_carrying_block:
					is_carrying_block = True
					robot_print_current_state('cozmo.robot.Robot.is_carrying_block: True')
			elif not robot.is_carrying_block:
				if is_carrying_block:
					is_carrying_block = False
					robot_print_current_state('cozmo.robot.Robot.is_carrying_block: False')

# event monitor: robot is localized (I don't think this is working right now)

			# if robot.is_localized:
				# if not is_localized:
					# is_localized = True
					# robot_print_current_state('cozmo.robot.Robot.is_localized: True')
			# elif not robot.is_localized:
				# if is_localized:
					# is_localized = False
					# robot_print_current_state('cozmo.robot.Robot.is_localized: False')				

# event monitor: robot is falling

			if robot.is_falling:
				if not is_falling:
					is_falling = True
					robot.stop_all_motors()
						
					cozmostate = 9
					robot_print_current_state('Switching to state 9 - Falling!')
					lightstate=0
			elif not robot.is_falling:
				if is_falling:
					is_falling = False
					cozmostate = 0
					lightstate=0
					robot_print_current_state('no longer falling switching to state 0')

# event monitor: robot moves onto charger

			if robot.is_on_charger:
				if not is_on_charger:
					is_on_charger = True
					#freeplay = 0
					cozmostate = 1
					# if robot.is_freeplay_mode_active:
						# #robot.enable_all_reaction_triggers(True)
						# robot.stop_freeplay_behaviors()
					# robot.abort_all_actions(log_abort_messages=True)
					# robot.wait_for_all_actions_completed()
					# robot.clear_idle_animation()
					# robot.stop_all_motors()
					robot_print_current_state('moved onto the charger')
					color1=cozmo.lights.Color(int_color=65535, rgb=None, name=None)
					light1=cozmo.lights.Light(on_color=color1)
					light2=cozmo.lights.Light(on_color=color1)
					light3=cozmo.lights.Light(on_color=color1)
					robot.set_backpack_lights(None, light3, light2, light1, None)
					# robot_set_backpacklights(65535)  # 65535 is blue
					lightstate = 0
					# #robot.play_anim_trigger(cozmo.anim.Triggers.Sleeping, loop_count=1, in_parallel=True, num_retries=0, ignore_body_track=True, ignore_head_track=False, ignore_lift_track=True).wait_for_completed()
					# try:
						# robot.play_anim_trigger(cozmo.anim.Triggers.GoToSleepGetIn).wait_for_completed()
					# except:
						# pass
					#robot_set_backpacklights(65535) # blue
					# if cozmostate==6:
						# try:
							# #robot.play_anim("anim_sparking_success_02").wait_for_completed()
							# robot_reaction_chance(cozmo.anim.Triggers.SparkSuccess,1,True,False,False)
						# except:
							# pass
						# try:
							# robot.set_head_angle(degrees(0)).wait_for_completed()
						# except:
							# pass
						# robot_print_current_state('docked')
					# try:
						# #robot.play_anim("anim_gotosleep_getin_01").wait_for_completed()
						# robot_reaction_chance(cozmo.anim.Triggers.GoToSleepGetIn,1,True,False,False)
					# except:
						# pass
					# try:
						# robot_reaction_chance(cozmo.anim.Triggers.CodeLabSleep,1,True,False,False)
						# #robot.play_anim("anim_gotosleep_sleeping_01").wait_for_completed()
					# except:
						# pass
					
					#robot.play_anim_trigger(cozmo.anim.Triggers.StartSleeping, loop_count=1, in_parallel=True, num_retries=0, ignore_body_track=True, ignore_head_track=False, ignore_lift_track=True).wait_for_completed()
					if robot.is_charging:
						cozmostate = 1
						robot_print_current_state('switching to state 1')
					else:
						cozmostate = 2
						maxbatvoltage = robot.battery_voltage
						robot_print_current_state('on charger, not charging')
					#print(msg)
			elif not robot.is_on_charger:
				if is_on_charger:
					#robot_set_backpacklights(16711935)  # 16711935 is green
					color1=cozmo.lights.Color(int_color=16711935, rgb=None, name=None)
					light1=cozmo.lights.Light(on_color=color1)
					light2=cozmo.lights.Light(on_color=color1)
					light3=cozmo.lights.Light(on_color=color1)
					is_on_charger = False
					cozmostate = 0
					robot_print_current_state('switching to state 0 - moved off charger')
					#print(msg)

# event monitor: robot has detected cliff

			if robot.is_cliff_detected and not robot.is_falling and not robot.is_picked_up and cozmostate != 6:
				if not is_cliff_detected:
					robot.stop_all_motors()
					is_cliff_detected = True
					wasinfreeplay = 0
					robot_print_current_state('cliff detected')
					#print(msg)
					if freeplay == 1:
						freeplay = 0
						wasinfreeplay = 1
						if robot.is_freeplay_mode_active:
							##robot.enable_all_reaction_triggers(True)
							robot.stop_freeplay_behaviors()
						#robot.wait_for_all_actions_completed()
					robot.abort_all_actions(log_abort_messages=True)
					robot.clear_idle_animation()
					try:
						robot.drive_wheels(-40, -40, l_wheel_acc=30, r_wheel_acc=30, duration=1.5)
					except:
						robot_print_current_state('failed to drive')
					try:
						robot.drive_wheels(-40, -40, l_wheel_acc=30, r_wheel_acc=30, duration=1.5)
					except:
						robot_print_current_state('failed to drive')
					is_cliff_detected = False
					robot_print_current_state('cliff no longer detected')
					#print(msg)
			elif not robot.is_cliff_detected:
				if is_cliff_detected:
					robot_print_current_state('switching from cliff mode')
					is_cliff_detected = False
					if wasinfreeplay == 1:
						freeplay = 1
						wasinfreeplay = 0
						if robot.is_freeplay_mode_active:
							##robot.enable_all_reaction_triggers(True)
							robot_print_current_state('re-enabling freeplay')
							robot.start_freeplay_behaviors()

# event monitor: robot is picking or placing something
			if robot.is_picking_or_placing:
				if not is_picking_or_placing:
					is_picking_or_placing = True
					msg = 'cozmo.robot.Robot.is_picking_or_placing: True'
					#print(msg)
					robot_print_current_state('Robot.is_picking_or_placing: True')
			elif not robot.is_picking_or_placing:
				if is_picking_or_placing:
					is_picking_or_placing = False
					msg = 'cozmo.robot.Robot.is_picking_or_placing: False'
					robot_print_current_state('Robot.is_picking_or_placing: False')
					#print(msg)		
				
# event monitor: robot is pathing (traveling to a target)
			if robot.is_pathing:
				if not is_pathing:
					is_pathing = True
					msg = 'cozmo.robot.Robot.is_pathing: True'
					#print(msg)
					robot_print_current_state('Robot.is_pathing: True')
			elif not robot.is_pathing:
				if is_pathing:
					is_pathing = False
					msg = 'cozmo.robot.Robot.is_pathing: False'
					robot_print_current_state('Robot.is_pathing: False')
					#print(msg)	

# event monitor (behavior is running)
			if robot.is_behavior_running:
				if not is_behavior_running:
					is_behavior_running = True
					msg = 'cozmo.robot.Robot.is_behavior_running: True'
					robot_print_current_state('Robot.is_behavior_running: True')
			elif not robot.is_behavior_running:
				if is_behavior_running:
					is_behavior_running = False
					msg = 'cozmo.robot.Robot.is_behavior_running: False'
					robot_print_current_state('Robot.is_behavior_running: False')

		
# event monitor: robot is moving
# too spammy/unreliable

			# if robot.is_moving:
				# if not is_moving:
					# is_moving = True
					# robot_print_current_state('Robot.is_moving: True')
			# elif not robot.is_moving:
				# if is_moving:
					# is_moving = False
					# robot_print_current_state('Robot.is_moving: False')	

# end of detection loop
			# # camera IR control
			# if camera.exposure_ms < 60:
				# # robot.say_text("light").wait_for_completed()
				# robot.set_head_light(False)
			# elif camera.exposure_ms >= 60:
				# # robot.say_text("dark").wait_for_completed()
				# robot.set_head_light(True)
			time.sleep(0.1)

def print_prefix(evt):
	msg = evt.event_name + ' '
	return msg

def print_object(obj):
	if isinstance(obj,cozmo.objects.LightCube):
		cube_id = next(k for k,v in robot.world.light_cubes.items() if v==obj)
		msg = 'LightCube-' + str(cube_id)
	else:
		r = re.search('<(\w*)', obj.__repr__())
		msg = r.group(1)
	return msg

def monitor_generic(evt, **kwargs):
	global robot,cozmostate,freeplay,msg,camera,objmsg,facemsg
	msg = print_prefix(evt)
	if 'behavior' in kwargs or 'behavior_type_name' in kwargs:
		msg += kwargs['behavior_type_name'] + ' '
		msg += kwargs['behavior'] + ' '
	elif 'obj' in kwargs:
		msg += print_object(kwargs['obj']) + ' '
	elif 'action' in kwargs:
		action = kwargs['action']
		if isinstance(action, cozmo.anim.Animation):
			msg += action.anim_name + ' '
		elif isinstance(action, cozmo.anim.AnimationTrigger):
			msg += action.trigger.name + ' '
	else:
		msg += str(set(kwargs.keys()))
	robot_print_current_state('generic monitor data incoming')

#
# event monitor: robot is experiencing unexpected movement
#
def monitor_EvtUnexpectedMovement(evt, **kwargs):
	global robot,cozmostate,freeplay,msg,camera
	msg = 'Unexpected Movement'
	robot_print_current_state('unexpected movement')
	#print(msg)
	if  cozmostate != 3 and cozmostate !=9 and cozmostate !=6:
		robot_print_current_state('unexpected behavior during action; aborting')
		#print("unexpected behavior during action; aborting")
		robot.abort_all_actions(log_abort_messages=True)
		robot.stop_all_motors()
		robot.wait_for_all_actions_completed()
		#print("unexpected behavior during action; aborting")
		robot_print_current_state('unexpected behavior during action; aborting')
		

#
# event monitor: an object was tapped
#
def monitor_EvtObjectTapped(evt, *, obj, tap_count, tap_duration, tap_intensity, **kwargs):
	msg = print_prefix(evt)
	msg += print_object(obj)
	msg += ' count=' + str(tap_count) + ' duration=' + str(tap_duration) + ' intensity=' + str(tap_intensity)
	robot_print_current_state('object tapped')
#
# event monitor: a face was detected
#
def monitor_face(evt, face, **kwargs):
	msg = print_prefix(evt)
	name = face.name if face.name is not '' else '[unknown face]'
	expression = face.expression if face.expression is not '' else '[unknown expression]'
	msg += name + ' face_id=' + str(face.face_id) + ' looking ' + str(face.expression)
	#print(msg)
	robot_print_current_state('face module')
#
# event monitor: an object started moving
#

def monitor_EvtObjectMovingStarted(evt, *, obj, acceleration, **kwargs):
	msg = print_prefix(evt)
	msg += print_object(obj) + ' '
	msg += ' accleration=' + str(acceleration)
	robot_print_current_state('object started moving')
	
#
# event monitor: an object stopped moving
#
def monitor_EvtObjectMovingStopped(evt, *, obj, move_duration, **kwargs):
	msg = print_prefix(evt)
	msg += print_object(obj) + ' '
	msg += ' move_duration ' + str(move_duration) + 'secs'
	#print(msg)
	robot_print_current_state('object stopped moving')
#
# event monitor: an object appeared in our vision
#
def monitor_EvtObjectAppeared(evt, **kwargs):
	global cozmostate,robot,freeplay,start_time,needslevel,scheduler_playokay,use_cubes, charger, lowbatvoltage, use_scheduler,msg, camera, foundcharger,bhvmsg,facemsg,objmsg
	msg = print_prefix(evt)
	msg += print_object(kwargs['obj']) + ' '
	if print_object(kwargs['obj']) == "Charger":
		charger = robot.world.charger
	if print_object(kwargs['obj']) == "Charger" and (cozmostate == 5 or cozmostate == 98):
		robot_print_current_state('FOUND THE CHARGER')
		#print("it's the charger and we're looking for it!")
		cozmostate = 6
		charger = robot.world.charger
	robot_print_current_state('object appeared')

dispatch_table = {
  
  cozmo.objects.EvtObjectTapped        : monitor_EvtObjectTapped,
  cozmo.objects.EvtObjectMovingStarted : monitor_EvtObjectMovingStarted,
  cozmo.objects.EvtObjectMovingStopped : monitor_EvtObjectMovingStopped,
  cozmo.objects.EvtObjectAppeared      : monitor_EvtObjectAppeared,
  cozmo.faces.EvtFaceAppeared          : monitor_face,
  cozmo.faces.EvtFaceDisappeared       : monitor_face,
  cozmo.robot.EvtUnexpectedMovement    : monitor_EvtUnexpectedMovement,

}


excluded_events = {	# Occur too frequently to monitor by default
	
	 cozmo.behavior.EvtBehaviorRequested,
	 cozmo.objects.EvtObjectDisappeared,
	 cozmo.faces.EvtFaceObserved,
	 cozmo.objects.EvtObjectObserved
	 
	 
}

def monitor(_robot, _q, evt_class=None):
	if not isinstance(_robot, cozmo.robot.Robot):
		raise TypeError('First argument must be a Robot instance')
	if evt_class is not None and not issubclass(evt_class, cozmo.event.Event):
		raise TypeError('Second argument must be an Event subclass')
	global robot
	global q
	global thread_running
	robot = _robot
	q = _q
	thread_running = True
	if evt_class in dispatch_table:
		robot.world.add_event_handler(evt_class,dispatch_table[evt_class])
	elif evt_class is not None:
		robot.world.add_event_handler(evt_class,monitor_generic)
	else:
		for k,v in dispatch_table.items():
			if k not in excluded_events:
				robot.world.add_event_handler(k,v)
	thread_is_state_changed = CheckState(1, 'ThreadCheckState', q)
	thread_is_state_changed.start()

def unmonitor(_robot, evt_class=None):
	if not isinstance(_robot, cozmo.robot.Robot):
		raise TypeError('First argument must be a Robot instance')
	if evt_class is not None and not issubclass(evt_class, cozmo.event.Event):
		raise TypeError('Second argument must be an Event subclass')
	global robot
	global thread_running
	robot = _robot
	thread_running = False

	try:
		if evt_class in dispatch_table:
			robot.world.remove_event_handler(evt_class,dispatch_table[evt_class])
		elif evt_class is not None:
			robot.world.remove_event_handler(evt_class,monitor_generic)
		else:
			for k,v in dispatch_table.items():
				robot.world.remove_event_handler(k,v)
	except Exception:
		pass
		
def robot_print_current_state(currentstate):
	global robot,needslevel,start_time,cozmostate,msg,highbatvoltage,maxbatvoltage,objmsg,facemsg,bhvmsg,lightstate,batlightcounter,debugging, batcounter
	if not batcounter:
		batcounter = 0
	if batcounter > 5:
		robot_backbackbatteryindicator()
		batcounter = 0
	batcounter += 1
	robot_set_needslevel()
	currentbehavior = robot.current_behavior
	if currentbehavior == None and cozmostate == 4:
		currentbehavior = 'freeplay'
	runtime = round(((time.time() - start_time)/60),2)
	"{:.2f}".format(runtime)
	if debugging==1:
		#logging.warn('istate %s' % cozmostate,'| battery %s' % str(round(robot.battery_voltage, 2)),'| energy %s' % round(needslevel, 2),'| anim %s' % str(robot.is_animating),'| behav %s' % str(robot.is_behavior_running),'| bkpk %s' % lightstate,'| state: %s' % str(currentstate),'| msg: %s' % str(msg))
		#print("state %s" % cozmostate,"| battery %s" % str(round(robot.battery_voltage, 2)),"| energy %s" % round(needslevel, 2),"| anim %s" % str(robot.is_animating),"| behav %s" % str(robot.is_behavior_running),"| bkpk %s" % lightstate,"| action: %s" % str(currentstate),"| msg: %s" % str(msg),"accel: %s" % str(robot.accelerometer))
		print("state %s" % cozmostate,"| battery %s" % str(round(robot.battery_voltage, 2)),"| energy %s" % round(needslevel, 2),"| anim %s" % str(robot.is_animating),"| behav %s" % str(robot.is_behavior_running),"| bkpk %s" % lightstate,"| action: %s" % str(currentstate),"| msg: %s" % str(msg),"curbehav: %s" % str(currentbehavior))
		
	else:
		os.system('cls' if os.name == 'nt' else 'clear')
		#
		# commented out thingies either didn't do what I expected or didn't work
		#
		#print("cozmo sees     : %s"  % str(robot.world.connected_light_cubes))
		#print("wheelie        : %s" % str(robot.PopAWheelie))
		#print("Cubes connected: %s" % robot.world.World.active_behavior.connected_light_cubes)
		#print("Behavior       : %s" % str(cozmo.behavior.Behavior))
		#print("Max Battery    : %s" % str(round(maxbatvoltage, 2)))
		#print("Max off charger: %s" % str(round(highbatvoltage, 2))) 
		#print("idle anim      : %s" % str(robot.is_animating_idle))
		#print("actions        : %s" % str(robot.has_in_progress_actions))
		# print("Object log     : %s" %objmsg)
		# print("Face log       : %s" %facemsg)
		# print("Behavior log   : %s" %bhvmsg)
		print("State          : %s" % str(currentstate))
		print("Internal state : %s" % str(cozmostate))
		print("Battery        : %s" % (str(round(robot.battery_voltage, 2))))
		print("Energy         : %s" % (round(needslevel, 2)))
		print("Runtime        : %s" % str(runtime))
		print("running behav  : %s" % (str(robot.is_behavior_running)))
		print("animating      : %s" % (str(robot.is_animating)))
		print("Event log      : %s" % str(msg))
		print("Lightstate     : %s" % str(lightstate))

	
#
# END OF EVENT MONITOR FUNCTIONS
#
# START THE SHOW!
#
cozmo.robot.Robot.drive_off_charger_on_connect = False
#
#uncomment the below line to load the viewer 
#cozmo.run_program(cozmo_unleashed, use_viewer=True)
#
# you may need to install a freeglut library, the cozmo SDK has documentation for this. If you don't have it comment the below line and uncomment the one above.
cozmo.run_program(cozmo_unleashed, use_viewer=True, use_3d_viewer=True)
# which will give you remote control over Cozmo via WASD+QERF while the 3d window has focus
#
# below is just the program running without any camera view or 3d maps
#cozmo.run_program(cozmo_unleashed)