#import sys
#sys.path.append("/Users/lpk/lpkmac/c1")
from math import *
import pickle

import utilities
reload(utilities)
from utilities import *

import Map
reload(Map)
from Map import *

sensorPoses = [(0.1,  0.175, 1.571),
               (0.17, 0.15,  1.13),
               (0.2,  0.11,  0.698),
               (0.225, 0.05, 0.262),
               (0.225, -0.05, -0.262),
               (0.2,  -0.11, -0.698),
               (0.17, -0.15, -1.13),
               (0.1, -0.175, -1.571)]

#sonarMax = 5.0  # value for longer sonarcycle
#sonarMax = 2.465 #value for sonarcycle=12ms; value in simulator

def writeReadings(worldMap, path, xmin, xmax, ymin, ymax, n, sonarMax = 100):
    print "Writing to ", path
    xstep = (xmax - xmin)/float(n)
    ystep = (ymax - ymin)/float(n)
    thetaMin = 0.0
    thetaMax = 2.0 * math.pi
    thetaStep = (thetaMax - thetaMin)/float(n)

    readings = make3DArray(n, n, n, 0.0)
    x = xmin
    for ix in range(n):
        print ix
        y = ymin
        for iy in range(n):
            th = thetaMin
            for ith in range(n):
                readings[ix][iy][ith] = \
                    idealSonarReadings((x,y,th), sensorPoses, worldMap,sonarMax)
                th += thetaStep
            y += ystep
        x += xstep
    print "pickling"
    f=open(path, 'w')
    pickle.dump(readings,f)
    f.close()

def sq(x):
    return x*x
def norm(vec):
    div = sqrt(float(sum(map(sq, vec))))
    return tuple(map(lambda x: x/div, vec))
def dot(veca, vecb):
    return sum([veca[i]*vecb[i] for i in range(len(veca))])
def vec(line):
    return (line[1][0]-line[0][0], line[1][1]-line[0][1])
def angle(linea, lineb):
    return acos(dot(norm(vec(linea)), norm(vec(lineb))))

# Args are robot pose and sensor pose
def idealSonarReadings((rx,ry,rth), sensorPoses, worldMap, sonarMaxRange):
    readings = []

    def intersection(linea, lineb):
        def helper(((xa, ya),(xb,yb)),((xc,yc),(xd,yd))):
            try:
                xa, ya, xb, yb, xc, yc, xd, yd = \
                    tuple(map(float, [xa, ya, xb, yb, xc, yc, xd, yd]))
                s = ((xb-xa)*(ya-yc)+(yb-ya)*(xc-xa))/\
                    ((xb-xa)*(yd-yc)-(yb-ya)*(xd-xc)) 
                t = ((xc-xa)+(xd-xc)*s)/(xb-xa)
                if s <= 1 and s >=0 and t <= 1 and t >= 0:
                    fromt = (xa+(xb-xa)*t,ya+(yb-ya)*t)
                    froms = (xc+(xd-xc)*s,yc+(yd-yc)*s)
                    if abs(fromt[0]-froms[0]) < 0.001 and \
                            abs(fromt[1]-froms[1]) < 0.001:
                        return fromt
                    else:
                        return False 
                else:
                    return False 
            except ZeroDivisionError:
                return False
        first = helper(linea, lineb)
        if first:
            return first
        else:
            return helper(lineb, linea)

    ### Main body
    for (sx,sy,sth) in sensorPoses:
        (gsx,gsy) = localToGlobal((rx,ry,rth),(sx,sy))
        sonarRay = ((gsx,gsy),
                    (gsx + sonarMaxRange*math.cos(rth+sth),
                     gsy + sonarMaxRange*math.sin(rth+sth)))
        goodHits = filter(lambda (x, y): x != False, \
                          [[intersection(seg, sonarRay), seg] for seg in worldMap.segs])
        if len(goodHits) > 0:
            minDist = 999999
            bestResult = []
            for (hit, seg) in goodHits:
                d = dist((gsx,gsy), hit)
                if d < minDist:
                    minDist = d
                    bestResult = (d, abs(math.pi/2.0 - angle(sonarRay, seg)))
            readings.append(bestResult)
        else:
            readings.append((sonarMaxRange, 0.0))
    return readings

outDir = "/afs/athena.mit.edu/user/t/l/tlp/Desktop/lab14/"

def writeDistAngle(outDir):
    writeReadings(Map(boxWorldBoxes), outDir + "distAngle/boxWorld20.dat", 
                  0.0, 3.05, 0.0, 3.05, 20)
    writeReadings(Map(emptySquareBoxes), 
                  outDir + "distAngle/emptySquare20.dat", 
                  0.0, 3.05, 0.0, 3.05, 20)

