# Test plotting distances at angle 
# WF 2019-06-13
from pi.servo import Servo
from pi.ultrasound import UltraSound 
import RPi.GPIO as GPIO
import pantograph
import math
import time

# a point at the angle and distance given
class DistPoint:
  def __init__(self,angle,dist):
     angle=angle-75
     self.x=math.sin(math.radians(angle))*dist;
     self.y=math.cos(math.radians(angle))*dist;
#
# plot the measured distances of the ultrasonic sensor
#
class DistancePlot(pantograph.PantographHandler):

   def setup(self):
        print("Starting angle/distance measurement ...")
        self.radius = min(self.width, self.height) / 2
        self.maxdist=150

        self.us1=UltraSound(15,14)
        self.us2=UltraSound(21,20)
        self.us1.start()
        self.us2.start()

        self.servo1 = Servo(23)
        self.servo2 = Servo(24)
        self.servo1.start()
        self.servo2.start()
        self.dpDict1={}
        self.dpDict2={}
        self.firstUpdate=True

   def ruler(self):
        for dist in range(10,self.maxdist+10,10):
          # draw the circle for the "rim" of the wheel
          self.draw_circle(self.width/2, 0, self.scaleY(dist), "#f0f0f0")

   def scaleX(self,x):
        return x*self.width/self.maxdist;
 
   def scaleY(self,y):
        return y*self.height/self.maxdist;

   def valid(self,dist):
      if dist>self.maxdist:
        return False;
      if dist<0:
        return False;
      return True;

   #
   # plot the distance of the given dist/angle
   #
   def plotDistance(self,xofs,dangle,dpDict,dist,angle,step,color):
     angle=angle+dangle
     prevangle=angle-step	
     # only plot valid distances
     if self.valid(dist):
       # create the point
       dp=DistPoint(angle,self.scaleY(dist))
       if prevangle in dpDict:
         dp2=dpDict.get(prevangle)
         xofs=self.width/2+self.scaleX(xofs)
         self.draw_line(self.width-(xofs+dp.x),dp.y,self.width-(xofs+dp2.x),dp2.y,color)
       dpDict[angle]=dp

   def update(self):
        if self.firstUpdate:
           self.ruler()
           self.firstUpdate=False
        step=1
        start=50
        stop=110+step
        meanCount=3
        dx=0
        dangle=7.5
        for angle in range(start,stop,step):
            pwm1=self.servo1.setAngle(angle)
            pwm2=self.servo2.setAngle(angle)
            dist1 = self.us1.meanDistance(meanCount)
            time.sleep(0.02) # avoid indirect echos          
            dist2 = self.us2.meanDistance(meanCount)
            self.plotDistance(dx,dangle,self.dpDict1,dist1,angle,step,"#ff8000")
            self.plotDistance(-dx,-dangle,self.dpDict2,dist2,angle,step,"#0000FF")
            print angle,"{0:.2f}".format(pwm1),self.us1.distAsString(dist1),self.us2.distAsString(dist2)
            # speed of servo is 0.1 secs per 60 degrees 
            time.sleep(0.1/6) # 

if __name__ == '__main__':
    try:
      app = pantograph.SimplePantographApplication(DistancePlot)
      app.run("0.0.0.0", 8000)  
    except KeyboardInterrupt:
        print("Aborted ultrasound distance measurement by User ... cleaning up GPIO")
        GPIO.cleanup()