#########################################################################
#
# Script: ascii_orbit.py
#
# Author: Igor Volobouev, Feb 2005
#
# Purpose: contains code for reading GLAST orbit and pointing history
#          from a file and performing orbit and attitude interpolations
#
#########################################################################

import math

from basic_kinematics import *
from timerep import *

class OrbitDatum:
    "Element of an orbit sequence."
    __xaxis = V3(1.0, 0.0, 0.0)
    __zaxis = V3(0.0, 0.0, 1.0)
    def __init__(self, t, posx, posy, posz, raz, decz, rax, decx, \
                 razenith, deczenith, lon, lat, alt):
        self.t   = Time(t)
        self.pos = V3(posx, posy, posz)
        self.rot = Rotation(rax, decx, raz, decz)
        self.zenith = dirFromRaDec(razenith, deczenith)
        self.lon = lon
        self.lat = lat
        self.alt = alt
    def __repr__(self):
        x = self.rot(OrbitDatum.__xaxis)
        z = self.rot(OrbitDatum.__zaxis)
        return str(self.t.dtime()) + ' ' + str(self.pos.x) + \
               ' ' + str(self.pos.y) + ' ' + str(self.pos.z) + \
               ' ' + str(z.ra()) + ' ' + str(z.dec()) + \
               ' ' + str(x.ra()) + ' ' + str(x.dec()) + \
               ' ' + str(self.zenith.ra()) + ' ' + str(self.zenith.dec()) + \
               ' ' + str(self.lon) + ' ' + str(self.lat) + ' ' + str(self.alt)
    def __cmp__(self, other):
        return cmp(self.t, other.t)

class AsciiOrbit(list):
    """Representation of GLAST orbit and pointing history.

Capable of reading and writing files in the format accepted by Gleam:
http://confluence.slac.stanford.edu/display/DC2/Orbit+and+Pointing+History

Intended usage:

o = AsciiOrbit()
forb = open(file_name, 'r')
o.read(forb)
forb.close()

An AsciiOrbit object can read several files sequentially."""
    __xaxis = V3(1.0, 0.0, 0.0)
    __zaxis = V3(0.0, 0.0, 1.0)
    def __init__(self, arg=None):
        if (arg is None):
            list.__init__(self)
        else:
            list.__init__(self, arg)
        self._sorted = 0
    # Override some list mutating methods so that
    # we always know whether the object is sorted.
    # Note that item removing methods (pop,
    # __delitem__, etc.) do not change the sorting
    # order.
    def append(self, x):
        list.append(self, x)
        self._sorted = 0
    def extend(self, x):
        list.extend(self, x)
        self._sorted = 0
    def insert(self, i, x):
        list.insert(self, i, x)
        self._sorted = 0
    def reverse(self):
        list.reverse(self)
        self._sorted = 0
    def __iadd__(self, other):
        list.__iadd__(self, other)
        self._sorted = 0
    def __imul__(self, other):
        list.__imul__(self, other)
        self._sorted = 0
    def __setitem__(self, key, value):
        list.__setitem__(self, key, value)
        self._sorted = 0
    def __setslice__(self, i, j, value):
        list.__setslice__(self, i, j, value)
        self._sorted = 0
    # For large orbits the sort method is very slow,
    # so we do not want to re-sort an already sorted
    # orbit.
    def sort(self):
        "Sorts the orbit data in the order of increasing time."
        if (not self._sorted):
            list.sort(self)
            self._sorted = 1
    # Read/write methods
    def read(self, f):
        "Reads the orbit data from the given file object."
        linenum = 0
        errorline = 0
        for line in f:
            linenum += 1
            data = list()
            items = line.split()
            if (len(items) == 13):
                for datum in items:
                    try:
                        data.append(float(datum))
                    except ValueError:
                        errorline = linenum
            else:
                errorline = linenum
            if (errorline): break
            self.append(apply(OrbitDatum, data))
        if errorline:
            raise SyntaxError, "failed to parse line " + \
                  str(errorline) + " in file " + f.name()
        self.sort()
    def write(self, f):
        "Writes sorted orbit data into the given file object."
        self.sort()
        for el in self:
            print >> f, el
    def closest(self, t, n):
        "Finds n orbit elements closest to the given time."
        nitems = len(self)
        if (n > nitems):
            raise ValueError, "not enough data"
        if (n < 0):
            raise ValueError, "number of elements can not be negative"
        if (n == 0):
            return list()
        self.sort()
        if (t <= self[0].t):
            return range(n)
        if (t >= self[-1].t):
            return range(nitems-n, nitems)
        lo = 0
        hi = nitems
        while lo < hi:
            mid = (lo+hi)//2
            if t < self[mid].t:
                hi = mid
            else:
                lo = mid+1
        tmp = [(abs(self[i].t - t), i) for i in \
               xrange(max(lo-n, 0), min(lo+n, nitems))]
        tmp.sort()
        tmp2 = [self[el[1]] for el in tmp[:n]]
        tmp2.sort()
        return tmp2
    def zenith_and_altitude(self, t):
        "Finds zenith direction and spacecraft altitude at the given time."
        self.sort()
        if (t < self[0].t or t > self[-1].t):
            raise ValueError, "time is outside the orbit range"
        o0, o1 = self.closest(t, 2)
        z0 = o0.zenith
        z1 = o1.zenith
        ang = z0.angle(z1)
        s = (t - o0.t).dtime()/(o1.t - o0.t).dtime()
        if (ang < 1.0e-8):
            # Use cartesian linear interpolation
            zt = (z0*(1.0 - s) + z1*s).direction()
        else:
            # Use spherical linear interpolation
            rot = Rotation(vprod3(z0, z1), ang*s)
            zt = rot(z0)
        alt = o0.alt*(1.0 - s) + o1.alt*s
        return (zt, alt)
    def latitude_and_longitude(self, t):
        "Finds spacecraft latitude and longitude at the given time."
        self.sort()
        if (t < self[0].t or t > self[-1].t):
            raise ValueError, "time is outside the orbit range"
        o0, o1 = self.closest(t, 2)
        # Use just the simple interpolation. Correct trajectory
        # reconstruction is the task of the orbit simulator.
        dir0 = dirFromLatLon(o0.lat, o0.lon)
        dir1 = dirFromLatLon(o1.lat, o1.lon)
        ang = dir0.angle(dir1)
        s = (t - o0.t).dtime()/(o1.t - o0.t).dtime()
        if (ang < 1.0e-8):
            dirt = (dir0*(1.0 - s) + dir1*s).direction()
        else:
            rot = Rotation(vprod3(dir0, dir1), ang*s)
            dirt = rot(dir0)
        return (dirt.dec(), dirt.ra())
    def orientation(self, t, deg=3):
        "Finds the spacecraft orientation at the given time."
        self.sort()
        if (t < self[0].t or t > self[-1].t):
            raise ValueError, "time is outside the orbit range"
        if (deg == 1):
            # Linear interpolation requested
            o0, o1 = self.closest(t, 2)
            s = (t - o0.t).dtime()/(o1.t - o0.t).dtime()
            return slerp(o0.rot, o1.rot, s)
        elif (deg == 3):
            # Cubic interpolation requested
            o0, o1, o2, o3 = self.closest(t, 4)
            ttot = (o3.t - o0.t).dtime()
            # Check that the orbit sampling is uniform in time
            s1 = (o1.t - o0.t).dtime()/ttot
            s2 = (o2.t - o1.t).dtime()/ttot
            s3 = (o3.t - o2.t).dtime()/ttot
            tol = 1.0e-5
            if (math.fabs(s1-1.0/3.0) > tol or \
                math.fabs(s2-1.0/3.0) > tol or \
                math.fabs(s3-1.0/3.0) > tol):
                raise ValueError, "uneven orbit sampling around t = " \
                                  + str(t.dtime())
            s = (t - o0.t).dtime()/ttot
            return scubic(o0.rot, o1.rot, o2.rot, o3.rot, s)
        else:
            raise ValueError, "bad interpolation degree"
    def rot_interpolation_uncertainty(self, t):
        "Estimates spacecraft orientation uncertainty due to interpolation."
        # This function returns a tuple in which the first
        # element is the estimated error in the pointing 
        # direction and the second element is the error
        # in yaw angle. The errors are estimated by comparing
        # linear and cubic orientation interpolations.
        # Of course, in reality the systematic uncertainty
        # due to miscalibration will probably dominate
        # any interpolation error.
        r1 = self.orientation(t, 1)
        x1 = r1(AsciiOrbit.__xaxis)
        z1 = r1(AsciiOrbit.__zaxis)
        r3 = self.orientation(t, 3)
        x3 = r3(AsciiOrbit.__xaxis)
        z3 = r3(AsciiOrbit.__zaxis)
        return (z1.angle(z3), x1.angle(x3 - z1*sprod3(x3, z1)))