import SimpleCV as scv

import numpy as np

import sys

import socket

from clist import CList

from speak import speak

from phoneImu import PhoneIMU

#import userinput2 as uinp

from userinput2 import UserInput

from directions2 import Directions

from getch2 import Getch, fetch

import datetime

import coordinates as coord

idxmap=["All", "Left", "Center", "Right", "Back"]

dirmap={"all":0, "left":1, "center":2, "right":3, "back":4}

#Output function

#def out(x): print x

out = lambda x: speak(x)

def log(arg):

return 0

print arg

class Hist():

return self.val

class Walk():

return -1

def getIPAddr():

return socket.gethostbyname(socket.getfqdn())

def readDest():

return end

def walkloop():

lastcall = datetime.datetime.now()

if __name__ == '__main__':

if len(sys.argv) > 1 and sys.argv[1] == "w":

walkloop()

wEn = False #walk

dEn = True #directions

iEn = True #imu-requires app

cEn = True #coordinates-requires app

#getch = Getch()

lastcall = datetime.datetime.now()

userInput = Hist()

ipaddr = getIPAddr()

nextStep = Hist() #next step

nextElbow = Hist() #(lat long) of next turning-point

here = Hist() #Current location (lat, long)

if wEn:

walk = Walk() #walking object

cam = walk.getcam()

#(1) Get destination

#end = readDest()

end = "220 Yonge St, Toronto, ON M5B2H1"

#(2) Get start location, i.e. current location

port = 5007

print "Listening for coordinates app at {}:{}".format(ipaddr, port)

coord = coord.Coordinates(ipaddr=ipaddr, port=port)

start = coord.getCoord()

#start = "40 St George St, Toronto, ON, M5S2E4"

print "Start is {}".format(start)

#(3) Get directions

directions = Directions(start, end)

steps = directions.getsteps()

print "Steps to {} are {}".format(end, steps)

#90 -> N

#180-> E

#270-> S

#0 | 360-> W

port = 5005

print "Listening for phoneIMU app at {}:{}".format(ipaddr, port)

imu = PhoneIMU(ipaddr, port)

dirc = Hist() #direction reading

rotcount = 99

rotwait = 100

rotcmd = ""

text = ""

print "Entering main loop"

while True:

if wEn:

depth = cam.getDepth()

depth.show()

if dirc.update(imu.getdirc3()):

log("Current user direction is {}".format(dirc.val))

#Update direction

dirc.update(imu.getdirc3())

if nextStep.update(directions.getsteps(start=coord.getCoord())[0]):

log("nextStep is {}".format(nextStep.val))

allSteps = directions.getStepCoords()

if len(allSteps) > 1 and nextElbow.update(allSteps[1]): #if path only has one step

log("nextElbow is {}".format(nextElbow.val))

if here.update(coord.getCoord(frmt=True)):

log("here is {}".format(here.val))

dist, brng = coord.getDistBrng(here.val, nextElbow.val)

log("distance from here to next elbow is {}, bearings are {}".format(dist, brng))

#Compare nextElbow with current bearing and rotate accordingly

if brng > dirc.val:

brngDiff = brng - dirc.val

if brngDiff > 45:

rotcmd="Rotate by {} degrees Counter Clockwise".format(brngDiff)

if rotcount >= rotwait:

out(rotcmd)

rotcount = 0

else:

rotcount +=1

else:

rotcmd = "Walk straight ahead"

else:

brngDiff = dirc.val - brng

if brngDiff > 45:

rotcmd="Rotate by {:10.2f} degrees Clockwise".format(brngDiff)

if rotcount >= rotwait:

out(rotcmd)

rotcount = 0

else:

rotcount +=1

else:

rotcmd = "Walk straight ahead"

#User input

ch = fetch() #getch()

if ch:

ch=ch[0]

print "User entered {}".format(ch)

if userInput.update(ch):

try:

#speak based on input

if userInput.val == "c": #current coordinates

text = "Current latitude is {:10.4f} and longitude is {:10.4f}".format(here.val[0], here.val[1])

elif userInput.val == "e": #end location

text = "End location is {}".format(end)

elif userInput.val == "d": #direction (compass heading)

text = "You are facing {}".format(olddirc)

elif userInput.val == "f": #next step

text = nextStep.val

elif userInput.val == "r":

text = rotcmd

elif userInput.val == "q": #exit

sys.exit(1)

if text:

out(text)

print "text is '{}'".format(text)

except Exception as e:

print e

if wEn:

ret = walk.walk() #get walkable side

if ret != -1:

d = idxmap[ret]

nav = None

if d != "Center":

if (datetime.datetime.now() - lastcall).seconds >2:

nav = "Go {}".format(d)

else:

nav = "Go center"

if nav:

out(nav)

lastcall = datetime.datetime.now()