#!/usr/bin/env python

__author__ = "Mike McCann"
__copyright__ = "Copyright 2012, MBARI"
__license__ = "GPL"
__maintainer__ = "Mike McCann"
__email__ = "mccann at mbari.org"
__doc__ = '''

Base classes for reading and writing data for the BEDs project

Mike McCann
MBARI 3 April 2014

@var __date__: Date of last svn commit
@undocumented: __doc__ parser
@author: __author__
@license: __license__
'''

import os
import sys
import csv
import time
import coards
import urllib2
import datetime
import numpy as np
from euclid import Quaternion
from pupynere import netcdf_file
from subprocess import Popen, PIPE
from util import quatern2euler, quaternConj, quatern2eulervector

class BEDS(object):
    '''
    Container for common methods to be reused by BEDs processing software.
    Initially used by bed2netcdf.py and bed2x3d.py to read the data.
    '''

    def __init__(self):
        '''
        Initialize arrays
        '''
        self.secList = []
        self.pSecList = []
        self.pBarList = []

        self.axList = []
        self.ayList = []
        self.azList = []
        self.quatList = []

        self.rxList = []
        self.ryList = []
        self.rzList = []

        self.pxList = []
        self.pyList = []
        self.pzList = []
        self.protList = []

        self.mxList = [0]
        self.myList = [0]
        self.mzList = [0]
        self.diffrotList = [0]

        self.latList = []
        self.lonList = []

    def readTrajectory(self):
        '''
        Read file specified with --trajectory option into lists of lats and lons
        '''
        for r in csv.DictReader(open(self.args.trajectory), delimiter=','):
            self.latList.append(float(r['latitude']))
            self.lonList.append(float(r['longitude']))

        self.traj_lat = self.latList
        self.traj_lon = self.lonList

    def cumtrapz(self, x, y):
        '''Returns indefinite integral of discrete data in y wrt x.  

        Test with:
            >> cumtrapz([0,.2,.4,.6,.8],[1:5])                              ! Matlab
            ans =
                     0    0.3000    0.8000    1.5000    2.4000

            print self.cumtrapz([.0,.2,.4,.6,.8], np.array([1,2,3,4,5]))    ! Python
            [ 0.   0.3  0.8  1.5  2.4]
        '''

        sumA = 0
        sumAList = [sumA]
        for i in range(len(y))[:-1]:
            A = (x[i+1] - x[i]) * (y[i] + y[i+1]) / 2.0
            sumA = sumA + A
            sumAList.append(sumA)

        return np.array(sumAList)

    def add_global_metadata(self):
        '''
        This is the main advantage of using a class for these methods.  This method uses the
        instance variables to write metadata specific for the data that are written.
        '''

        iso_now = datetime.datetime.now().isoformat()

        self.ncFile.title = ''
        self.ncFile.netcdf_version = '3.6'
        self.ncFile.Conventions = 'CF-1.6'
        self.ncFile.date_created = iso_now
        self.ncFile.date_update = iso_now
        self.ncFile.date_modified = iso_now
        self.ncFile.featureType = self.featureType
        if self.featureType == 'trajectory':
            self.ncFile.geospatial_lat_min = np.min(self.latitude[:])
            self.ncFile.geospatial_lat_max = np.max(self.latitude[:])
            self.ncFile.geospatial_lon_min = np.min(self.longitude[:])
            self.ncFile.geospatial_lon_max = np.max(self.longitude[:])
            self.ncFile.geospatial_lat_units = 'degree_north'
            self.ncFile.geospatial_lon_units = 'degree_east'

            self.ncFile.geospatial_vertical_min= np.min(self.depth[:])
            self.ncFile.geospatial_vertical_max= np.max(self.depth[:])
            self.ncFile.geospatial_vertical_units = 'm'
            self.ncFile.geospatial_vertical_positive = 'down'

        self.ncFile.time_coverage_start = coards.from_udunits(self.time[0], self.time.units).isoformat()
        self.ncFile.time_coverage_end = coards.from_udunits(self.time[-1], self.time.units).isoformat()

        self.ncFile.distribution_statement = 'Any use requires prior approval from the MBARI BEDS PI: Dr. Charles Paull'
        self.ncFile.license = self.ncFile.distribution_statement
        self.ncFile.useconst = 'Not intended for legal use. Data may contain inaccuracies.'
        self.ncFile.history = 'Created by "%s" on %s' % (' '.join(sys.argv), iso_now,)

    def pCount2Bar(self, count):
        '''
        Simple hard coding of Bob & Denis adc.c adcEngValue() function to convert pressure in counts to engineering units,
        in this case bars.  From adc.c and decode.c:

            #define VREF            2.50
            #define RAW_TO_VOLTS(n)     ((float)(n) * VREF / 65536.0)


            typedef struct
            {
                Flt32   a;
                Flt32   b;
                char    *units;
            } ADConversion;
            
            MLocal ADConversion adConv[] =
            { {12.11, 0.0, "Batt Volts"},  {12.11, 0.0, "Batt Volts"},
              {35.0, 8.75, "bar"}, {50.0, 12.5, "psia"},
              {100.0, 50.0, "degC"}, {47.175, 23.82, "%"},
              {1.0, 0.0, "Volts"}
            };
            
              dcEngValue(Nat16 chan, Nat16 counts, char **unitp)
            {
                if (chan >= NumberOf(adConv))
                chan = HUMIDITY_CHAN + 1;
            
                           if (unitp != NULL)
                   *unitp = adConv[chan].units;
            
               return((adConv[chan].a * RAW_TO_VOLTS(counts)) - adConv[chan].b);
            
            } /* adcEngValue() */
        '''

        return 35.0 * (count * 2.50 / 65536.0)  - 8.75 

    def readBEDsFile(self, infile):
        '''
        Open @infile, read values, apply offsets and scaling.
        Return numpy arrays of the acceleration and rotation data as originally recorded.

        Output from 'decode -d <file>' looks like:

            FileHdr ver=1 ID=0 rate=3000 2013/02/11 09:23:53.700 dur=26.614
            Second Marker 1360574634 2013/02/11 09:23:54
            Ext Pressure 0 counts  0.00 volts
            Inertial Data  acc:  0.00397  0.00249  0.03131  quat:  0.99365  0.00409 -0.01617  0.11078
            Inertial Data  acc:  0.00137  0.00266  0.03006  quat:  0.99365  0.00415 -0.01624  0.11078

        Note: A special version of decode.c exists in this directory with printf customizations beyond
        what Bob initially wrote.  This is a stopgap solution until this python script can be modified 
        to read the records directly.  We want to eventually do this modification so that this script
        can work everywhere.

        This method may be called multiple times in order to append data from multiple .EVT files.
        '''

        self.rateHz = None
        esec = None

        # Use Bob's decode program and pipe its output into our parser
        self.infile = infile
        p1 = Popen(["./decode", "-d", infile], stdout=PIPE)
        for line in p1.stdout:
            if self.args.verbose > 1: 
                    print line

            if line.startswith('FileHdr'):
                # FileHdr ver=1 ID=0 rate=3000 2013/02/11 09:23:53.700 dur=26.614
                # convert cycles/min to Hz
                self.rateHz = float(line.split()[3].split('=')[1]) / 60.0

            if line.startswith('Second Marker'):
                # Second Marker 1360574634 2013/02/11 09:23:54
                # Note: the 'Second Marker' is jittery, compare with accumulated time (esec) from the beginning
                if not esec:
                    esec = float(line.split()[2])
        

                esec_diff = abs(float(line.split()[2]) - esec)
                if self.args.verbose > 0:
                    print 'esec_diff =', esec_diff

                if esec_diff > 2:
                    print "WARNING: esec_diff is greater than 2", esec_diff

            if line.startswith('Ext Pressure'):
                #Ext Pressure 48014 counts  1.83 volts
                counts = float(line.split()[2])
                self.pSecList.append(esec)
                self.pBarList.append(self.pCount2Bar(counts))
                

            if esec and line.startswith('Inertial'):
                acc = line.split(':')[1].split()[:3]

                # Time and accelerations
                self.secList.append(esec)
                self.axList.append(float(acc[0]))
                self.ayList.append(float(acc[1]))
                self.azList.append(float(acc[2]))

                # Rotations as quaternions
                qs = line.split(':')[2].split()[:4]
                self.quatList.append( (float(qs[0]), float(qs[1]), float(qs[2]), float(qs[3])) )

                esec += 1 / self.rateHz
                if self.args.verbose > 1:
                    print esec

        # Bail out if no pressure data - event must be at least a second in duration
        if len(self.pSecList) == 0:
            raise Exception('No pressure data in file %s' % infile)

        # Make the Lists numpy arrays so that we can do matrix math operations; they have units of seconds and bar
        self.s = np.array(self.secList)
        self.s2013 = self.s - 1356998400.0      # (date(2013,1,1)-date(1970,1,1)).total_seconds() in python2.7
        self.ps = np.array(self.pSecList)
        self.ps2013 = np.array(self.pSecList) - 1356998400.0
        self.pr = np.array(self.pBarList)

    def processAccelerations(self):
        '''
        For member component accelerations produce additional useful member lists
        '''
        self.ax = np.array(self.axList)
        self.ay = np.array(self.ayList)
        self.az = np.array(self.azList)

        self.a = np.sqrt(self.ax**2 + self.ay**2 + self.az**2)

    def processRotations(self, useMatlabCode=False):
        '''
        For member quatList of quaternion tuples produce additional useful member lists for graphical display and analysis
        '''
        # "Quaternions came from Hamilton after his really good work had been done; and, though beautifully ingenious, 
        # have been an unmixed evil to those who have touched them in any way, including Clerk Maxwell." - Lord Kelvin, 1892.

        # If readBEDsFile() processes multiple BED files then subsequent self.quatLists will include concatenated data, initialize appended to lists
        self.rxList = []
        self.ryList = []
        self.rzList = []

        self.pxList = []
        self.pyList = []
        self.pzList = []
        self.protList = []
   
        self.mxList = [0]
        self.myList = [0]
        self.mzList = [0]
        self.diffrotList = [0]

        mx = my = mz = 0
        diffrot = 0
        diffrot_sum = 0
        for i, quat in enumerate(self.quatList):
            if self.args.verbose > 1:
                print "quat = ",  quat

            if useMatlabCode:
                # Convert to Euler angles using same code that Brian's Matlab conversion uses
                quat = quaternConj(quat)
                xRot, yRot, zRot = quatern2euler(quat)
            else:
                q = Quaternion(*quat)
                xRot, yRot, zRot = q.get_euler()

            if useMatlabCode:
                # Convert to Euler Vectors using SpinCalc code
                px, py, pz, prot = quatern2eulervector(quat)
            else:
                q = Quaternion(*quat)
                try:
                    prot, v = q.get_angle_axis()
                except ValueError as e:
                    print e
                    print 'WARNING: Using previous quaternion at index %d' % i
                    q = Quaternion(*last_quat)
                    prot, v = q.get_angle_axis()

                px = v.x
                py = v.y
                pz = v.z

            self.rxList.append(xRot)
            self.ryList.append(yRot)
            self.rzList.append(zRot)

            self.pxList.append(px)
            self.pyList.append(py)
            self.pzList.append(pz)
            self.protList.append(prot)
       
            # Compute first difference (division) to get rotation rate
            if i > 0:
                dq = Quaternion(*quat) * Quaternion.conjugated(Quaternion(*last_quat))
                if useMatlabCode:
                    mx, my, mz, diffrot = quatern2eulervector([dq.w, dq.x, dq.y, dq.z])
                    ##diffrot = diffrot - 180         # Seems to be 180 deg off ??
                else:
                    try:
                        diffrot, v = dq.get_angle_axis()
                    except ValueError as e:
                        print e
                        print 'WARNING: Using previous quaternion difference at index %d' % i
                        diffrot, v = last_dq.get_angle_axis()

                    mx = v.x
                    my = v.y
                    mz = v.z

                self.mxList.append(mx)
                self.myList.append(my)
                self.mzList.append(mz)
                self.diffrotList.append(diffrot)
                if self.args.verbose > 1:
                    print "diffrot = ",  diffrot

                diffrot_sum += diffrot
                last_dq = dq
            
            last_quat = quat

            if self.args.verbose:
                fmtStr = "%2d. xRot, yRot, zRot, diffrot, diffrot_sum = %7.2f %7.2f %7.2f %7.2f %7.2f"
                fmtStr += "  px, py, pz, prot = %7.2f %7.2f %7.2f %7.2f"
                fmtStr += "  mx, my, mz = %6.2f %6.2f %6.2f"
                print fmtStr % (i, xRot, yRot, zRot, diffrot, diffrot_sum, px, py, pz, prot, mx, my, mz)


        self.rx = np.array(self.rxList)
        self.ry = np.array(self.ryList)
        self.rz = np.array(self.rzList)

        self.mx = np.array(self.mxList)
        self.my = np.array(self.myList)
        self.mz = np.array(self.mzList)

        self.diffrot = np.array(self.diffrotList)

        # Rate of rotation in deg/sec
        self.rotrate = np.absolute(self.diffrot * self.rateHz * 180.0 / np.pi)

        # Cumultative rotation count
        self.rotcount = np.cumsum(np.absolute(self.diffrot)) / 2. / np.pi