#! /usr/bin/env python

#
# Version History:
#
# Version 1.0:  Initial Release.  17 Oct 2012.
#

import sys
import numpy
from pylab import *
import scipy
import matplotlib.pyplot as plt

matplotlib.rc('xtick', labelsize=16) 
matplotlib.rc('ytick', labelsize=16) 



# Plot the node/sensor locations with their node numbers
def plotLocs(nodeLocs):

    plt.plot(nodeLocs[:,0], nodeLocs[:,1], '.', markersize=14.0)

    #Increase the axes to show full map.
    xmin, xmax, ymin, ymax = plt.axis()
    deltay          = ymax-ymin
    epsy            = deltay*0.1
    deltax          = xmax-xmin
    epsx            = deltax*0.1
    plt.axis((xmin-epsx, xmax+epsx, ymin-epsy, ymax+epsy))
    for number, coord in enumerate(nodeLocs):
        plt.text(coord[0], coord[1]+ epsy*0.2, str(number), 
             horizontalalignment='center', verticalalignment='bottom', fontsize=16)
    plt.xlabel('X Coordinate (m)', fontsize=18)
    plt.ylabel('Y Coordinate (m)', fontsize=18)
    plt.grid()

    
# Purpose: Determine the coordinate at a given time.
# Inputs:
#  t_ms: a time in ms
#  pivotCoords: a list of coordinates a person may "hit"
#  pathInd: the indices of the row in pivotCoords that the person hits them
#  startPathTime: the time at which the person hits his first pivot point
#  speed:  in pivot points "hit" per ms
def calcActualPosition(t_ms, pivotCoords, pathInd, startPathTime, speed):
    endPathTime = startPathTime + (len(pathInd)-1)/speed
    if (t_ms < startPathTime) or (t_ms >= endPathTime):
        actCoord  = []
    else:
       point_real = (t_ms-startPathTime) * speed
       point_int  = floor(point_real)
       point_frac = point_real - point_int
       prevCoord  = pivotCoords[pathInd[point_int],:]
       nextCoord  = pivotCoords[pathInd[point_int + 1],:]    
       actCoord   = prevCoord*(1-point_frac) + nextCoord*point_frac
    return actCoord

# Calculate the penalized RMSE between the actualCoords and the estCoords
def prmse(actualCoord, estCoord, noPersonKey, penalty):
    rows  = max(1.0, actualCoord.shape[0])
    sumSE = 0.0;
    eps   = 0.01;
    # Loop here line by line and add in the penalized error
    for i, ac in enumerate(actualCoord):
        noP = abs(ac[0] - noPersonKey) < eps
        estNoP = abs(estCoord[i,0] - noPersonKey) < eps
        if noP and not(estNoP):
            sumSE += penalty
        elif estNoP and not(noP):
            sumSE += penalty
        else:
            sumSE += sum((ac - estCoord[i,:])**2)
            
    return sqrt(sumSE/rows)



penalty       = 4.0**2.
noPersonKey   = -99.0
pivotFileName = 'Exam2_9_pivot_coords.txt'
pivotCoords   = numpy.loadtxt(pivotFileName)
pathFileName  = 'Exam2_9_path.txt'  # list of pivot numbers in order.
pathInd       = numpy.loadtxt(pathFileName)
startPathTime = 28000.0 *(60.0/70.0)  # ms
speed         = 1.0 / (2000*60.0/70.0) # pivot points per millisecond.
eps           = 0.01

coordFileName = 'my_house_coords.txt'  
sensorCoords  = numpy.loadtxt(coordFileName)

if len(sys.argv) >= 2:
    estFileName      = sys.argv[1]
else:
    print("Error: no estimated coordinate filename given.")

dataFileName  = 'Exam2_9.txt'
dataMat       = numpy.loadtxt(dataFileName)
[rows,cols]   = dataMat.shape
time_ms       = dataMat[:,-1]

estCoord      = numpy.loadtxt(estFileName)
if (estCoord.shape[0] != rows):
    print("Error: estimate coordinate file is not the correct number of rows")
if (estCoord.shape[1] != 2):
    print("Error: estimate coordinate file is not the correct number of columns")


offsetList    = arange(-2000.0,1001.0,250)
offsets       = len(offsetList)
actualCoord   = []
err           = zeros(len(offsetList))
for j,offset in enumerate(offsetList):
    for i in range(rows):
        actualCoord.append(zeros((rows, 2)))
        temp = calcActualPosition(time_ms[i] + offset, pivotCoords, 
                                     pathInd, startPathTime, speed)
        if len(temp)==0:
            actualCoord[j][i,:] = [noPersonKey, noPersonKey]
        else:
            actualCoord[j][i,:] = temp
    
    #print str(actualCoord-estCoord) 
    err[j]  = prmse(actualCoord[j], estCoord, noPersonKey, penalty)

pause

print str(err)
bestOI = argmin(err)
minPRMSE = err[bestOI]

print str(minPRMSE) + " pRMSE at offset = " + str(offsetList[bestOI])

plt.ion()
fig = plt.figure(num=1, figsize=(7, 7))
plt.cla()
plotLocs(sensorCoords)
#imhandle = plt.imshow(image, interpolation='none', origin='lower', 
#                      extent=imageExtent, vmin=0, vmax=8)
plt.ylabel('Y Coordinate (ft)')
plt.xlabel('X Coordinate (ft)')
step = 3
for i in range(0, rows, step):
    ac = actualCoord[bestOI][i,:]
    ec = estCoord[i,:]
    noP = abs(ac[0] - noPersonKey) < eps
    estNoP = abs(ec[0] - noPersonKey) < eps
    if not(noP) and not(estNoP):
        text(ac[0], ac[1],'X', horizontalalignment='center',
             verticalalignment='center', color='k')
        text(ec[0], ec[1],'o', horizontalalignment='center',
             verticalalignment='center', color='k')
        plot([ac[0], ec[0]], [ac[1], ec[1]],'r')
    elif not(noP) and estNoP:
        text(ac[0], ac[1],'X', horizontalalignment='center',
             verticalalignment='center', color='r')
    elif noP and not(estNoP):
        text(ec[0], ec[1],'o', horizontalalignment='center',
             verticalalignment='center', color='r')

plt.axes().set_aspect('equal', 'datalim')
#axis([-0.5, 25., -0.5, 24])
plt.draw()

outfname = estFileName[:-4] + "2.png"
print outfname
fig = fig.savefig(outfname)

raw_input()