import crData;
import pyCreeper;
import crGraphs;
import constants;

import numpy as np;
import matplotlib.pyplot as plt;



dataPath = "../cpp_observer/launch/infoEvents.txt"
#dataPath = constants.DATA_PATH+"/static_wsr2500/NR5_FOR_heap3-14000_LB_commD12500/run5_infoEvents.txt"
infoEventsData = crData.fileToArray(dataPath, "\t", True);


worksiteXs = [];
worksiteYs = [];
worksiteIds = [];

completedWorksiteXs = [];
completedWorksiteYs = [];

xMin=-9370;
yMin=-6800;
xMax=9850;
yMax=7760;

robotSize = 750; #-- robot has 7.5cm in diameter, which is 750 units
heapFreeSpace = 2*robotSize;
xMinHeap = xMin + heapFreeSpace;
xMaxHeap = xMax - heapFreeSpace;
yMinHeap = yMin + heapFreeSpace;
yMaxHeap = yMax - heapFreeSpace;

hasDepletedWorksites = False;
for i in range(len(infoEventsData)):
    eventName = infoEventsData[i][1];
    if (eventName == "E_SITE_ADDED"):
        worksiteX = infoEventsData[i][4];
        worksiteY = infoEventsData[i][5];
        worksiteXs.append(worksiteX);
        worksiteYs.append(worksiteY);
        worksiteIds.append(infoEventsData[i][3]);

        if (('heap1' in dataPath or 'heap3' in dataPath) and (worksiteY < yMinHeap or worksiteY > yMaxHeap or worksiteX < xMinHeap or worksiteX > xMaxHeap)):
            print("!!! worksite % outside of heap boundaries".format(i));

    elif (eventName == "E_SITE_COMPLETED"):
        hasDepletedWorksites = True;
        worksiteX = infoEventsData[i][4];
        worksiteY = infoEventsData[i][5];
        completedWorksiteXs.append(worksiteX);
        completedWorksiteYs.append(worksiteY);

    elif (eventName == 'ROBOT_BROKE'):
        print("!!! has robot broken")

crGraphs.DPI=100
#pyCreeper.crGraphs.createLinePlot([worksiteXs], worksiteYs);

fig = crGraphs.createScatterPlot(worksiteXs,worksiteYs,xLimMin_=xMin,xLimMax_=xMax,yLimMin_=yMin,yLimMax_=yMax, dataLabels_=worksiteIds, holdFigure_=True);
if (hasDepletedWorksites):
    fig = crGraphs.createScatterPlot(completedWorksiteXs,completedWorksiteYs,xLimMin_=xMin,xLimMax_=xMax,yLimMin_=yMin,yLimMax_=yMax, figure_=fig, colors_=['r' for x in range(len(completedWorksiteXs))], holdFigure_=True);

#-- draw heap boundaries
crGraphs.createScatterPlot([xMinHeap,xMaxHeap,xMinHeap,xMaxHeap],[yMinHeap,yMinHeap,yMaxHeap,yMaxHeap],figure_=fig,marker_='s',colors_=['k' for i in range(4)],xLimMin_=xMin,xLimMax_=xMax,yLimMin_=yMin,yLimMax_=yMax,);

