from Device5 import Device
import time
import hcsr04sensor.sensor as sensor

RHIP = 0
RKNEE = 1
RANKLE = 2

LHIP = 3
LKNEE = 4
LANKLE = 5 

class RoboMove(object):
 def __init__(self):
     self.ra = 0
     self.la = 0
     self.rh = 0
     self.lh = 0
     self.rk = 0
     self.lk = 0
     self.stop = True
     self.servo = Device("/dev/ttyAMA0","/dev/ttyAMA0")
     self.centralise()
     self.last=""
     #self.lean_left()

 def is_stop(self):
  return self.stop

 def go(self):
  print("go")
  return True

 def update_position(self):

  self.ra = self.servo.get_position(RANKLE)
  self.la = self.servo.get_position(LANKLE)
  self.rh = self.servo.get_position(RHIP)
  self.lh = self.servo.get_position(LHIP)
  self.rk = self.servo.get_position(RKNEE)
  self.lk = self.servo.get_position(LKNEE)


 def lean_left(self):
  self.update_position()
  print ("lean_left")
  for i in range(0,150,15):
   self.servo.set_target(LANKLE, self.la+i)
   self.servo.set_target(RANKLE, self.ra+int(i*1.2))
   self.servo.set_target(LKNEE, self.lk-int(i*0.7))
  self.servo.set_target(LANKLE, 1356) #1356
  self.servo.set_target(RANKLE, 1249) #1239
  self.servo.set_target(LKNEE, 1395) #1409
  return("lean_left")

 def lean_left2(self):
  self.update_position()
  print ("lean_left2")
  for i in range(0,90,15):
   self.servo.set_target(LANKLE, self.la+i)
   self.servo.set_target(RANKLE, self.ra+int(i*1.2))
   self.servo.set_target(LKNEE, self.lk-int(i*0.7))

  self.servo.set_target(LANKLE, 1356)
  self.servo.set_target(RANKLE, 1249)
  self.servo.set_target(LKNEE, 1395)
  return("lean left")

 def reverse_lean_left(self):
  self.servo.set_target(LANKLE, 1356)
  self.servo.set_target(RANKLE, 1249)
  self.servo.set_target(LKNEE, 1395)

  self.update_position()
  print ("reverse lean left")
  for i in range(0,-90,-10):
   self.servo.set_target(LANKLE, self.la+i)
   self.servo.set_target(RANKLE, self.ra+int(i*1.2))
   self.servo.set_target(LKNEE, self.lk-int(i*0.7))
  return("reverse lean left")

 def swing_right_leg_forward(self):
  print("swing right leg forward")
  self.update_position()
  for i in range(0,270,15):
   self.servo.set_target(RHIP, self.rh+i)
   self.servo.set_target(RKNEE, self.rk-int(i*0.5))
   self.servo.set_target(LHIP, self.lh+int(i*0.6))
   self.servo.set_target(LANKLE, self.la+int(i*0.1))
  return("swing right leg forward")


 def reverse_swing_right_leg_forward(self):
  print("reverse swing right leg forward")
  self.update_position()
  for i in range(0,-270,-10):
   self.servo.set_target(RHIP, self.rh+i)
   self.servo.set_target(RKNEE, self.rk-int(i*0.5))
   self.servo.set_target(LHIP, self.lh+int(i*0.6))
   self.servo.set_target(LANKLE, self.la+int(i*0.1))
  return("reverse swing right leg forward")


 def settle_right_leg_forward(self):
  print("settle right leg forward")  
  self.update_position()
  for i in range(0,-140,-15):
   self.servo.set_target(RHIP, self.rh+i)
   self.servo.set_target(LANKLE, self.la+i)
   self.servo.set_target(RANKLE, self.ra+int(i*1.2))
   self.servo.set_target(RKNEE, self.rk+int(i*0.5))
   self.servo.set_target(LKNEE, self.lk+int(i*0.5))
  return("settle right leg forward")


 def swing_left_leg_forward(self):
  print("swing left leg forward")
  self.update_position()
  #self.servo.setAngle(LANKLE, 90)
  for i in range(0,230,15):
   self.servo.set_target(LHIP, self.lh-int(i*0.5))
   self.servo.set_target(LKNEE, self.lk+int(i*0.3))
   self.servo.set_target(RHIP, self.rh-int(i*1))
   self.servo.set_target(RANKLE, self.ra-int(i*0.3))
  return("swing left leg forward")

 def reverse_swing_left_leg_forward(self):
  print("reverse swing left leg forward")
  self.update_position()
  for i in range(0,-230,-10):
   self.servo.set_target(LHIP, self.lh-int(i*0.5))
   self.servo.set_target(LKNEE, self.lk+int(i*0.3))
   self.servo.set_target(RHIP, self.rh-int(i*1))
   self.servo.set_target(RANKLE, self.ra-int(i*0.3))
  return("reverse swing left leg forward")



 def settle_left_leg_forward(self):
  print("settle left leg forward")
  self.update_position()
  for i in range(0,-150,-15):
   #self.servo.set_target(LHIP, self.lh+i)

   self.servo.set_target(LANKLE, self.la-i)
   self.servo.set_target(RANKLE, self.ra-int(i*1))
   self.servo.set_target(RKNEE, self.rk-int(i*0.1))
   self.servo.set_target(LKNEE, self.lk-int(i*0.5))
  return("settle swing left leg forward")


 def swing_left_leg_centre(self):
  self.update_position()
  for i in range(0,-180,-15):
   self.servo.set_target(RKNEE, self.rk+i)
   self.servo.set_target(LHIP, self.lh-i)


 def balance_left(self):
  print("balance shift left")
  self.update_position()

  self.servo.setAngle(LHIP,85)
  self.servo.setAngle(RHIP,95)
  time.sleep(0.2)
  self.servo.setAngle(RKNEE,95)
  self.servo.setAngle(RANKLE,95)
  self.servo.setAngle(LKNEE,90)
  self.servo.setAngle(LANKLE,85)
  #time.sleep(0.5)
#  for i in range(0, 10, 1):
#   self.servo.setAngle(RKNEE,100+int(i))
#   self.servo.setAngle(RANKLE,85-i)
#   self.servo.setAngle(LANKLE,85-i)
  return("balance shift left")

 def balance_right(self):
  print("balance shift right")
  self.update_position()

  self.servo.setAngle(RHIP,95)
  self.servo.setAngle(LHIP,85)
  time.sleep(0.2)
  self.servo.setAngle(RKNEE,90)
  self.servo.setAngle(RANKLE,85)
  self.servo.setAngle(LKNEE,80)
  self.servo.setAngle(LANKLE,100)

  #for i in range(0, 10, 1):
  # self.servo.setAngle(LKNEE,80-int(i))
  # self.servo.setAngle(RANKLE,85+i)
  # self.servo.setAngle(LANKLE,100+i)
  return("balance shift right")


# def lean_right(self):
#  print("lean right")
#  self.update_position()
#  for i in range(0,10,1):
#   self.servo.setAngle(RKNEE,100+int(i))
#   self.servo.setAngle(LANKLE, 90-int(i*1))
#   self.servo.setAngle(RANKLE, 90-int(i*1))
  #servo.set_target(RKNEE, rk+i*1
#  return(b"lean right")
 def lean_right(self):
  self.update_position()
  print ("lean right")
  print (self.ra) 
  print (self.la)
  print (self.rk)

  for i in range(0,85,15):
   self.servo.set_target(RANKLE, self.ra-i)
   self.servo.set_target(LANKLE, self.la-int(i*1.2))
   self.servo.set_target(RKNEE, self.rk+int(i*0.7))

  self.servo.set_target(RANKLE, 1000)
  self.servo.set_target(LANKLE, 1050)
  self.servo.set_target(RKNEE, 1400)
  return("lean right")


 def reverse_lean_right(self):
  print("reverse lean right")
  self.update_position()
  self.servo.set_target(RANKLE, 1000)
  self.servo.set_target(LANKLE, 1050)
  self.servo.set_target(RKNEE, 1400)
  self.update_position()
  for i in range(0,-85,-10):
   self.servo.set_target(LANKLE, self.la-int(i*1))
   self.servo.set_target(RANKLE, self.ra-int(i*1.2))
   self.servo.set_target(RKNEE, self.rk+int(i*0.7))

  return("reverse lean right")



 def centralise(self):
  print("centralise")
  self.balance_right()
  self.balance_left()
  time.sleep(0.2)
  self.lean_left()
  time.sleep(0.2)
  self.reverse_lean_left()
  time.sleep(0.2)
  self.lean_right()
  time.sleep(0.2)
  self.reverse_lean_right()
  time.sleep(0.2)
  self.stopp()

  return("centralise")
 def stopp(self):
  print("stopping")
  #self.lean_centre()
  self.servo.setAngle(LHIP,85)
  self.servo.setAngle(RHIP,95)
  time.sleep(0.2)
  self.servo.setAngle(RKNEE,90)
  self.servo.setAngle(RANKLE,90)
  self.servo.setAngle(LKNEE,90)
  self.servo.setAngle(LANKLE,90)
  return("stopping")

 
 def move2(self, data):
   print(data)
   return(data)

 def distance(self): 
   '''Example script using hcsr04sensor module for Raspberry Pi'''
   trig_pin = 9
   echo_pin = 11
   unit = 'metric'  # choices (metric or imperial)
   temperature = 20  # Celcius for metric, Fahrenheit for imperial
   round_to = 1  # report a cleaner rounded output.

   #  Create a distance reading with the hcsr04 sensor module
   value = sensor.Measurement(trig_pin, echo_pin, temperature, unit, round_to)
   raw_measurement = value.raw_distance()

   # Calculate the distance in centimeters
   metric_distance = value.distance_metric(raw_measurement)
   print ("The Distance = {} centimeters".format(metric_distance))
   return(metric_distance)

 def move(self, data):
   d = ""
   if (data=="forward0"):
     distance = self.distance()
     if(distance<=100):
       d=("Approaching a barrier:{}cm. Turn around now! ".format(distance))     
     if(distance<=30):
       data="stop"
       return ("halt")
     d+=self.lean_left()
   elif(data=="forward1"):
     d=self.swing_right_leg_forward()
   elif(data == "forward2"):
     d=self.settle_right_leg_forward()
   elif(data == "forward3"):
     d=self.balance_left()
   elif(data=="forward4"):
     distance = self.distance()
     if(distance<100):
       d=("Approaching a barrier:{}cm. Turn around now! ".format(distance))
     if(distance<=30):
       data="stop"
       return ("halt")
     d+=self.lean_right()

   elif(data== "forward5"):
     d=self.swing_left_leg_forward()
   elif(data == "forward6"):
     d=self.settle_left_leg_forward()
   elif(data == "forward7"):
     d=self.balance_right()
   elif(data=="forward8"):
     distance = self.distance()
     if(distance<100):
       d=("Approaching a barrier:{}cm. Turn around now! ".format(distance))
     if(distance<=30):
       data="stop"
       return ("halt")

     d+=self.lean_left2()
   elif(data=="left0"):
     self.lean_left()
     data="forward0"
     d="Rotate left"
   elif(data=="left1"):
     self.swing_right_leg_forward()
     data="forward1"
     d="Rotate left"
   elif(data=="left2"):  
     self.settle_right_leg_forward()
     data="forward2"
   elif(data=="left3"):
     self.balance_left()
     d="Rotate left"
   elif(data=="left4"):
     self.lean_right()
     data="forward4"
     d="Rotate left"
   elif(data=="left5"):
     self.reverse_lean_right()
     d="Rotate left"
   elif(data=="left6"):
     self.stopp()
     d="Rotate left"
   elif(data=="right0"):
     self.lean_right()
     d="Rotate right"
   elif(data=="right1"):
     self.swing_left_leg_forward()
     data="forward5"
     d="Rotate right"
   elif(data=="right2"):
     self.settle_left_leg_forward()
     data="forward6"
     d="Rotate right"
   elif(data=="right3"):
     self.balance_right()
     d="Rotate right"
   elif(data=="right4"):
     self.lean_left()
     d="Rotate right"
   elif(data=="right5"):
     self.reverse_lean_left()
     d="Rotate right"
   elif(data=="right6"):
     self.stopp()
     d="Rotate right"

   elif(data=="stop"):
     time.sleep(0.5)
     if(self.last=="forward1"):
       d=self.reverse_swing_right_leg_forward()
       d+=","+self.reverse_lean_left()
     elif(self.last == "forward2"):#when leg settle
       d=self.balance_left()
       d+=","+self.lean_right()
       d+=","+self.reverse_lean_right()
     elif(self.last == "forward3"):
       d=self.lean_right()
       d+=","+self.reverse_lean_right()
     elif(self.last=="forward4"):
       d=self.reverse_lean_right()
     elif(self.last=="forward5"):
       d=self.reverse_swing_left_leg_forward()
       d+=","+self.reverse_lean_right()
     elif(self.last == "forward6"):#settle
       d=self.balance_right()
       d+=","+self.lean_left2()
       d+=","+self.reverse_lean_left()
     elif(self.last == "forward7"):#balance shift
       d=self.lean_left2()
       d+=","+self.reverse_lean_left()
     elif(self.last=="forward8" or self.last=="forward0"):
       d=self.reverse_lean_left()
     elif(self.last=="stop"):
       d=self.stopp()
     else:
       d=self.stopp()
     d+=","+self.stopp()
     
   else:
     d=data

   self.last = data

   return (d)