###############################################################################
# Copyright 2020 - 2022 Trimble Inc.
# $Id: curve_fit_collector.py,v 1.3 2022/10/26 19:24:53 acartmel Exp $
# $Source: /home/wlentz/cvs_stinger/GPSTools/NoPiLive/curve_fit_collector.py,v $
###############################################################################

""" Ephemeris Curve Fit Collector
    Thread class to collect ephemeris curve fit data.
    The curve fit data is obtained by requesting the data from an active
    GNSS receiver and parsing the XML response.
"""

import time
import threading
import xml.etree.cElementTree as cET
import requests
from RXTools import SendHttpPost

###############################################################################

def _calc_cclk(dtime, cclk):
    """ Evaluate the curve-fit clock for the requested time. """
    return cclk[0] + (dtime*cclk[1]) + (dtime*dtime*cclk[2])

def _calc_cxyz(dtime, xyz):
    """ Evaluate the curve-fit axis for the requested time. """
    return xyz[0] + (dtime*xyz[1]) + (dtime*dtime*xyz[2]) + (dtime*dtime*dtime*xyz[3])

def _xml_array(xml, arrname, size):
    """ Translate numbered XML list (cx0, cx1, cx2 etc.) to a list. """
    ret = [0.0] * size
    for idx in range(size):
        ret[idx] = float(xml.find(arrname+str(idx)).text)
    return ret

def _xml_cfit(xml):
    """ Translate a single XML response and return the curve-fit data. """
    svid = int(xml.find('svid').text)
    sat_type = int(xml.find('sat_type').text)
    t_cf = float(xml.find('t_cf').text)
    w_cf = float(xml.find('w_cf').text)
    cfitx = _xml_array(xml, 'cx', 4)
    cfity = _xml_array(xml, 'cy', 4)
    cfitz = _xml_array(xml, 'cz', 4)
    cclk = _xml_array(xml, 'af', 3)
    cfit = CurveFitData(svid, sat_type, (w_cf, t_cf), (cfitx, cfity, cfitz), cclk)
    return cfit

class CurveFitData():
    """ Class to hold the curve fit data. """
    def __init__(self, svid, sat_type, t_cf, cxyz, cclk):
        self.svid = svid
        self.sat_type = sat_type
        self.t_cf = t_cf
        self.cxyz = cxyz
        self.cclk = cclk

    def calc_dtime(self, t_cf):
        """ Calculate the time difference between the curve fit and the
            requested time.
        """
        dtime = (t_cf[1] - self.t_cf[1]) + 7.0*86400.0*(t_cf[0] - self.t_cf[0])
        return dtime

    def calc_cclk(self, t_cf):
        """ Evaluate the curve-fit for the requested time.
            (X, Y, Z)
        """
        dtime = self.calc_dtime(t_cf)
        cclk = _calc_cclk(dtime, self.cclk)
        return cclk

    def calc_cfit(self, t_cf):
        """ Evaluate the curve-fit for the requested time.
            (X, Y, Z)
        """
        dtime = self.calc_dtime(t_cf)
        cfitx = _calc_cxyz(dtime, self.cxyz[0])
        cfity = _calc_cxyz(dtime, self.cxyz[1])
        cfitz = _calc_cxyz(dtime, self.cxyz[2])
        return (cfitx, cfity, cfitz)


###############################################################################

class CurveFitCollectorThread(threading.Thread):
    """ Thread class to collect ephemeris curve fit data. The thread
        periodically requests the curve-fit data from the receiver. The XML
        respond is collected.
        ip_addr = IP address of the receiver (str)
        user / pword = user name and password to access web interface (str)
        req_rate_s = request rate in [s] (int)
    """
    def __init__(self, ip_addr=None, user='admin', password='password', req_rate=30):
        threading.Thread.__init__(self)
        self.ip_addr = ip_addr
        self.user = user
        self.password = password
        self.req_rate = req_rate
        self.xmlreq = '/xml/dynamic/curveFits.xml'
        self.curve_fits = []
        self.run_thread = True
        self.timeout = 60.0
        if ip_addr is None or req_rate == 0:
            self.run_thread = False

    def _find_best_cfit(self, weeknum, tow, svid, sat_type):
        """ Return the best cfit for requested SV.
            Or None if nothing found.
            sat_type = Stinger SAT_TYPE
        """
        best = None
        mindt = self.timeout
        for cfit in self.curve_fits:
            if cfit.svid != svid or cfit.sat_type != sat_type:
                continue

            dtime = abs(cfit.calc_dtime((weeknum, tow)))
            if dtime < mindt and dtime < self.timeout:
                best = cfit
                mindt = dtime
        return best

    def num_cfits(self):
        """ Return the number of curve fits decoded. """
        return len(self.curve_fits)

    def del_all_cfits(self):
        """ Delete all curve-fit records. """
        self.curve_fits = []

    def del_cfit(self, idx):
        """ Delete curve-fit record for index idx. """
        if 0 <= idx < len(self.curve_fits):
            self.curve_fits.remove(idx)

    def get_all_cfits(self):
        """ Return all of the cfit data. """
        return self.curve_fits

    def get_cfit(self, idx):
        """ Return the cfit for idx. """
        if 0 <= idx < len(self.curve_fits):
            return self.curve_fits[idx]
        return None

    def best_cfit(self, weeknum, tow, svid, sat_type):
        """ Return the best cfit for requested SV.
            Or None if nothing found.
            tow = ToW [s]
            sat_type = Stinger SAT_TYPE
        """
        return self._find_best_cfit(weeknum, tow, svid, sat_type)

    def quit_thread(self):
        """ Call to stop the thread. Used for clean shut-down. """
        self.run_thread = False

    def run(self):
        """ Call to run the thread. The thread will run until:
            - the IP socket closes
            - quit_thread() is called
        """
        lasttime = 0.0
        while self.run_thread:
            # Use small sleep period to respond quicker to stop the thread
            currtime = time.time()
            if currtime - lasttime < self.req_rate:
                time.sleep(1)
                continue

            lasttime = currtime

            try:
                cfit_xml = SendHttpPost(self.ip_addr, self.xmlreq, self.user, self.password)
                xmlroot = cET.fromstring(cfit_xml)
                for xmlcfit in xmlroot:
                    cfit = _xml_cfit(xmlcfit)
                    self.curve_fits.append(cfit)
            except requests.exceptions.HTTPError:
                print("Failed to grab the curve fit data")
            print("Num CFits "+str(self.num_cfits()))
        print("Quit CFits "+str(self.num_cfits()))


###############################################################################

def main():
    """ Run one instance of the curve fit collector. """
    collector = CurveFitCollectorThread(ip_addr='10.1.150.75', req_rate=2)
    collector.start()
    cntr = 30
    while cntr:
        print("Num CFits "+str(collector.num_cfits()))
        cntr -= 1
        time.sleep(1)
    collector.quit_thread()

if __name__ == "__main__":
    main()