# -*- 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 or other target.
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

#"""

import numpy as np
import matplotlib.pyplot as plt
import utm #(Ivan)
from LSU import position_target_LSU
from matplotlib.backends.backend_pdf import PdfPages

#old paths in L shape
#filename = 'hotspotRange-20150622.log'
#filename = 'hotspotRange-20150615.log'

#First day nearshore
#filename = 'ranges_201603211753.log' #Square
#filename = 'ranges_201603211708.log' #Circle r100
#filename = 'ranges_201603211642.log' #1/4 Circle r400
#targetlat = 35.1373
#targetlon = -122.988

# secon day nearshore
#filename = 'ranges_201603231739.log'#Circle r300
#targetlat = 36.81382
#targetlon = -121.82515

# third day deep water (SES Station M)
#filename = 'ranges_201604132100.log' #circle d100 (r200)

# forth day deep water (benthic rover)
#filename = 'ranges_201604141948.log' #circle d500 (r200)
#filename = 'ranges_201604142127.log' #circle r400
#filename = 'ranges_201604142200.log' #circle r200

#500528.517989, 3888511.82956, 3977.77465161
#500523.717329, 3888508.90321, 3977.26288928

#500536.893855, 3888509.41746, 3979.52761256
#500508.995410, 3888517.43763, 3972.62070055

#Logs from BIN
targetlat = 36.701708
targetlon = -122.09384
targetdepth= 1836.7

#filename = 'ranges_201604251655.log' #circle r800
#filename = 'ranges_201604251936.log' #circle r100
#filename = 'ranges_201604252021.log' #circle d500 r400
#filename = 'ranges_201604252131.log' #circle d1000 r400

#filename = 'ranges_201609262340.log' # CPF ranges r400
filename = 'ranges_201609271718.log' # CPF ranges r800


#
#Logs from BIN Day 2
#filename = 'ranges_201605021855.log' #circle r400 it couldn't arrive at final point because it moves too slow (without propeller)
#filename = 'ranges_201605022036.log' #circle r400
#filename = 'ranges_201605022142.log' #square s400 it couldn't arrive at final point because it moves too slow for the currents (with propeller)

#Logs from BIN Day 3
#filename = 'ranges_201605051556.log' #square s800
#filename = 'ranges_201605052017.log' #circle r400
#filename = 'ranges_201605052124.log' #Triangle s800

#Logs form benthic Rover (Station M)
#filename = 'ranges_201607111610.log' #circle r800 


distancebp = 0

##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)


##Convert latlon to utm
tuple = utm.from_latlon(targetlat, targetlon)   
te, tn, zonenumber, zoneletter = tuple

teasting = 5.80937178e+05
tnorthing = 4.06217761e+06
targetdepth = 1.86139696e+03


latt=[]
lont=[]
for i in range(easting.size):
    #Convert latlon from utm
    tuple = utm.to_latlon(easting.item(i), northing.item(i), 10, 'S')   
    lat, lon = tuple
    latt.append(lat)
    lont.append(lon)
    

Plsu,eastingpoints,northingpoints,numpoints = position_target_LSU(filename,distancebp)

#Convert latlon from utm
tuple = utm.to_latlon(Plsu.item(0), Plsu.item(1), 10, 'S')   
Plsulat, Plsulon = tuple


"""plot altitude and longitude path of WG"""
"""with color"""
plt.figure(figsize=(5,5))
#plt.plot(easting,northing,'b-',lw=2,alpha=0.5,label='r = 800 m')
#plt.plot(easting2,northing2,'r-',lw=2,alpha=0.5,label='r = 100 m')
#plt.plot(easting3,northing3,'g-',lw=2,alpha=0.5,label='r = 400 m')

#Plsu0=np.matrix([500000.,3887829.4,3970.]).T
#plt.plot(Plsu0[0],Plsu0[1],'y^',label='Initial',alpha=0.7,markersize=12)
#plt.plot(Plsu2[0],Plsu2[1],'r^',label='Loc1',alpha=0.7,markersize=12)
plt.plot(Plsu[0],Plsu[1],'g^',label='Loc2',alpha=0.7,markersize=12)

#plt.text(Plsu0[0]+50,Plsu0[1]-150,'11/08/2015',fontweight='bold')
#plt.text(Plsu2[0]+50,Plsu2[1]+50,'04/14/2016',fontweight='bold')
#plt.text(Plsu[0]+50,Plsu[1]+50,'07/11/2016',fontweight='bold')


#plt.text(Plsu0[0]+50,Plsu0[1]-250,'(35.133333,-123)',fontweight='bold')
#plt.text(Plsu2[0]+50,Plsu2[1]-50,'(35.139463,-122.994250)',fontweight='bold')
#plt.text(Plsu[0]+50,Plsu[1]-50,'(35.141826,-122.992201)',fontweight='bold')

#plt.scatter(easting2, northing2, c=rang2, cmap='RdYlBu',s=80,alpha=0.7,vmin=4000,vmax=4140)
plt.scatter(easting, northing, c=rang, cmap='RdYlBu',s=80,alpha=0.7)

#plt.scatter(easting3, northing3, c=rang3, cmap='RdYlBu',s=80,alpha=0.7,vmin=4008,vmax=4062)
#plt.plot(easting2,northing2,'r--',lw=2,label='Test1')
plt.plot(easting,northing,'g--',lw=2,label='Test2')

#plt.text(581395, 4062800,'1000m',size=13)
#plt.text(581195, 4062500,'500m',size=13)
#plt.text(580959, 4062300,'0m',size=13)

#plt.plot(eastingpoints,northingpoints,'bo')
#plt.plot(teasting,tnorthing,'g^')

a=plt.colorbar()
a.set_label('Range (m)',size=12)
##Limits for different radius circles
#plt.xlim(580195,581680)
#plt.ylim(4061270,4063067)
#plt.xticks(np.arange(580195, 581780, 300))
#Limits for diffenrent distances circles
#plt.xlim(499800,501700)
#plt.ylim(4061671,4062662)
#plt.xticks(np.arange(499800, 501600, 500))
#plt.subplots_adjust(left=0.16)
#plt.subplots_adjust(bottom=0.18)
#plt.subplots_adjust(right=0.95)
#plt.legend(numpoints=1, loc='upper lefft',fontsize = 12,ncol=1)
#plt.legend(bbox_to_anchor=(0., 1.02, 1., .102), loc=3,
#           ncol=2, mode="expand", borderaxespad=0.,numpoints=1)
#plt.legend(bbox_to_anchor=(1.05, 1), loc=2, borderaxespad=0.)
plt.xlabel('Easting (m)',size=12)
plt.ylabel('Northing (m)',size=12)
#plt.title('WG paths and ranges',size=13)
#pp = PdfPages('benthicRover.pdf')
#plt.savefig(pp, format = 'pdf')
plt.show()
#pp.close()


print ('Target position: ('+str(Plsu.item(0))+', '+str(Plsu.item(1))+', '+str(Plsu.item(2))+')')
print ('Easting error = '+str(teasting-Plsu.item(0))+'m')
print ('Northing error = '+str(tnorthing-Plsu.item(1))+'m')
print ('Depth error = '+str(targetdepth-Plsu.item(2)))


rangbefore=rang #we save the original values for the last plot
eastingbefore=easting
northingbefore=northing

#Convert utm TO latlon
tuple = utm.to_latlon(Plsu.item(0), Plsu.item(1), zonenumber,zoneletter)   
lat, lon = tuple

print('\nTarget position is lat:'+str(lat)+' lon:'+str(lon)+' depth:'+str(Plsu.item(2)))

#easting_error=teasting-easting
#northing_error=tnorthing-northing
#range_ideal=np.sqrt(easting_error**2+northing_error**2)
#range_ideal=np.sqrt(range_ideal**2+targetdepth**2)
#range_error=range_ideal-rang
#plt.plot(range_error)
#plt.plot()


#plt.figure(figsize=(6,6))
#plt.plot(eastingc,northingc,'bo',label='circle')
#plt.plot(eastingt,northingt,'ro',label='triangle')
#plt.plot(eastings,northings,'yo',label='square')
#plt.plot(Plsu.item(0),Plsu.item(1),'kx',mew=3,markersize=9)
#plt.xlabel('Easting (m)',size=13)
#plt.ylabel('Northing (m)',size=13)
#plt.subplots_adjust(left=0.16)
#plt.subplots_adjust(bottom=0.18)
#plt.subplots_adjust(right=0.95)
#plt.legend(numpoints=1)
##plt.grid(True)
##plt.savefig('testSES_4252016.png', dpi = 1000)
#
#
##plt.title('Standard desviation')
##plt.ylim(0,50)
##plt.xlim(0,50)
#plt.show() 

#
#