From 5471f77e89b1d4464195e5323410d0638d86aa42 Mon Sep 17 00:00:00 2001
From: HappyZ <zhuyanzi@gmail.com>
Date: Thu, 8 Feb 2018 15:32:43 -0600
Subject: [PATCH] add option to plot location

---
 libLocalization.py | 60 +++++++++++++++++++++++++++++++---------------
 libMeasurement.py  | 35 +++++++++++++++++++++++----
 2 files changed, 72 insertions(+), 23 deletions(-)

diff --git a/libLocalization.py b/libLocalization.py
index 8785391..51c8c1a 100755
--- a/libLocalization.py
+++ b/libLocalization.py
@@ -129,25 +129,16 @@ def trilateration2d(mydict, bounds=None, verbose=False):
         tmp = loc.split(',')
         p = Point(tmp[0], tmp[1])
         points.append(p)
-        circles.append(Circle(p, mydict[loc][0]))
-    # print(len(points), len(circles))
+        if mydict[loc][1]:
+            for r in arange(
+                max(mydict[loc][0] - mydict[loc][1], 0.001),
+                mydict[loc][0] + mydict[loc][1],
+                1
+            ):
+                circles.append(Circle(p, r))
+        else:
+            circles.append(Circle(p, mydict[loc][0]))
     inner_points = calcInnerPoints(circles, bounds)
-    if len(inner_points) is 0:
-        # if empty, create multiple circles based on std
-        circles = []
-        for loc in mydict:
-            tmp = loc.split(',')
-            p = Point(tmp[0], tmp[1])
-            if mydict[loc][1]:
-                for r in arange(
-                    max(mydict[loc][0] - mydict[loc][1], 0.01),
-                    mydict[loc][0] + mydict[loc][1],
-                    10
-                ):
-                    circles.append(Circle(p, r))
-            else:
-                circles.append(Circle(p, mydict[loc][0]))
-        inner_points = calcInnerPoints(circles, bounds)
     if verbose:
         print('* Inner points:')
         if len(inner_points) is 0:
@@ -182,6 +173,15 @@ def deriveLocation(args, results):
     return loc
 
 
+def plotLocation(loc):
+    handler = None
+    try:
+        handler = plt.scatter(loc[0], loc[1])
+        plt.pause(0.01)
+    except Exception:
+        pass
+    return handler
+
 if __name__ == '__main__':
     loc = deriveLocation(
         {
@@ -202,4 +202,26 @@ if __name__ == '__main__':
             '34:f6:4b:5e:69:1e': (50, 50)
         }
     )
-    print(loc)
+    flagPlot = False
+    try:
+        import matplotlib.pyplot as plt
+        flagPlot = True
+    except Exception:
+        pass
+    if flagPlot:
+        fig = plt.figure()
+        plt.ion()
+        plt.xlim([-100, 300])
+        plt.ylim([-10, 500])
+        while 1:
+            try:
+                handler = plotLocation(loc)
+                if handler is None:
+                    plt.close(fig)
+                    break
+                handler.remove()
+            except KeyboardInterrupt:
+                plt.close(fig)
+                break
+            except Exception:
+                raise
diff --git a/libMeasurement.py b/libMeasurement.py
index c83fc6f..7c1f2d7 100755
--- a/libMeasurement.py
+++ b/libMeasurement.py
@@ -197,6 +197,18 @@ def wrapper(args):
             }
         }
     counter = 1
+    if args['plot']:
+        try:
+            import matplotlib.pyplot as plt
+            from libLocalization import plotLocation
+            args['plot'] = False
+            handler = None
+            fig = plt.figure()
+            plt.ion()
+            plt.xlim([-100, 500])
+            plt.ylim([-10, 500])
+        except Exception:
+            print('Cannot plot because lacking matplotlib!')
     with Measurement(
         args['interface'],
         ofp=args['filepath'], cali=args['cali']
@@ -221,6 +233,18 @@ def wrapper(args):
                         '* Derived location: ({0:.3f}, {1:.3f})'
                         .format(loc[0], loc[1])
                     )
+                    if args['plot']:
+                        try:
+                            if handler is not None:
+                                handler.remove()
+                            handler = plotLocation(loc)
+                            if handler is None:
+                                plt.close(fig)
+                        except KeyboardInterrupt:
+                            plt.close(fig)
+                            break
+                        except Exception:
+                            raise
             except KeyboardInterrupt:
                 break
             except Exception as e:
@@ -279,10 +303,13 @@ def main():
         '--locs',
         default=False,
         action="store_true",
-        help=(
-            "if set, derive location" +
-            "and store it to file"
-        )
+        help="if set, derive location and store it to file"
+    )
+    p.add_argument(
+        '--plot',
+        default=False,
+        action="store_true",
+        help="if set, will plot the derived location in realtime"
     )
     try:
         args = vars(p.parse_args())