# -*- coding: utf-8 -*-
"""
Created on Fri Jan 15 10:08:12 2016

@author: Ivan Masmitja
"""

"""
This script is to reed the raw data from a old mision using a Wave Glider
to geolocate a Benthos ROV at 4000m deepth in Monterrey Bay.
The raw data was saved in a .txt file as follows:

epochSec   target   easting       northing   range utmZone
1434411335,ROVER,501300.302506,3888683.19713,4054.0,10

#"""

def position_target_LSU(filename,distancebp):
    """LS-U target positioning algorithm
        Variables (filename,distancebp)
        Return (target(x,y,z),eastingpoints,northingpoints)"""

    import numpy as np
    import matplotlib.pyplot as plt

#    filename = 'hotspotRange-20150622.log'
    #filename = 'hotspotRange-20150615.log'
    ##print "Which file you would open?"
    ##filename = raw_input("> ")
    
    """open the file into a float number and copy all columns in the varables"""
    epochSec, easting, northing, rang, utmZone = np.loadtxt(filename,skiprows=1, delimiter=',', 
                                                            usecols=(0,2,3,4,5), unpack=True)
                                                            
                                                            
                                                            
    """epochSec to time in seconds"""
    epochSec=epochSec-np.amin(epochSec)
    """Transform center of coordinates to minimum value"""
    eastingoffset = easting[0]
    northingoffset = northing[0]
    easting = easting - eastingoffset
    northing = northing - northingoffset
    
    
    
    """find the index of northing and easting arrays where they are 100m of distances between points"""
#    distancebp=2 #we want 100m between points
    northindex=[]
    #    northindex.append(0)
    n=0
    a=0
    for i in range(0, northing.size-1):
        #print (np.abs(northing[a]-northing[i]))
        if np.abs(northing[a]-northing[i]) >= distancebp:
            northindex.append(i)
            n+=n
            a=i
    
    eastindex=[]
    n=0
    a=0
    for i in range(0, easting.size-1):
        #print (np.abs(northing[a]-northing[i]))
        if np.abs(easting[a]-easting[i]) >= distancebp:
            eastindex.append(i)
            n+=n
            a=i
    
    
    indexpoints = list(set(northindex + eastindex))
#    indexpoints = np.array(northindex + eastindex)
    
    """Points selected as anchor nodes"""
    eastingpoints=easting[[indexpoints]]
    northingpoints=northing[[indexpoints]]
    rangepoints=rang[[indexpoints]]
    numpoints=eastingpoints.size
    
    
    """Unconstrained Least Squares (LS-U) algorithm 2D"""
    """/P_LS-U = N0* = N(A^T A)^-1 A^T b"""
    """where:"""
    #points in matrix form
    P=np.matrix([eastingpoints,northingpoints])
    # N is:
    N = np.concatenate((np.identity(2),np.matrix([np.zeros(2)]).T),axis=1)
    # A is:
    A = np.concatenate((2*P.T,np.matrix([np.zeros(numpoints)]).T-1),axis=1)
    # b is:
    b = np.matrix([np.diag(P.T*P)-rangepoints*rangepoints]).T
    # Then using the formula "/P_LS-U" the position (x,y) of the target is:
    Plsu = N*(A.T*A).I*A.T*b
    # Finally we calculate the depth (z) as follows
    r=np.matrix(np.power(rangepoints,2)).T
    a=np.matrix(np.power(Plsu[0]-eastingpoints,2)).T
    b=np.matrix(np.power(Plsu[1]-northingpoints,2)).T
    depth = np.sqrt(r-a-b)
    depth = np.mean(depth)
    Plsu = np.concatenate((Plsu.T,np.matrix(depth)),axis=1).T
        
    #Adjust the x,y points with offset
    Plsu[0]=Plsu[0]+eastingoffset
    Plsu[1]=Plsu[1]+northingoffset
    easting = easting + eastingoffset
    northing = northing + northingoffset
    eastingpoints=eastingpoints+eastingoffset
    northingpoints=northingpoints+northingoffset
    
#    
#    n=50
#    """Plot the final picture"""
#    fig=plt.figure(figsize=(5,5))
#    plt.plot(easting,northing,'bo',eastingpoints,northingpoints,'ro',Plsu.item(0),Plsu.item(1),'yo') 
#    plt.annotate(Plsu, xy=(Plsu.item(0)+20,Plsu.item(1)+20))
#    plt.xlim(np.amin(easting)-n,np.amax(easting)+n)
#    plt.ylim(np.amin(northing)-n,np.amax(northing)+n)
#    plt.xlabel('easting (m)')
#    plt.ylabel('northing (m)')
#    plt.title('Wave Glider path')
#    plt.show() 
#    """ Target position for (hotspotRange-20150622.log) file"""
#    MBARI_northing = 3888242.
#    MBARI_easting = 500671.
#    MBARI_depth = 3980.
#    easting_error = MBARI_easting-Plsu.item(0)
#    northing_error = MBARI_northing-Plsu.item(1)
#    depth_error = MBARI_depth-Plsu.item(2)
#    print('Easting error (LS-U) = %.1fm' %easting_error)
#    print('Northing error (LS-U)) = %.1fm' %northing_error)
#    print('Depth error (LS-U)) = %.1fm' %depth_error)
    return(Plsu,eastingpoints,northingpoints,numpoints)