# Copyright (C) 2021 Eric Martin
# 
# This file is part of oi-ctd.
# 
# oi-ctd is free software: you can redistribute it and/or modify
# it under the terms of the GNU General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
# 
# oi-ctd is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU General Public License for more details.
# 
# You should have received a copy of the GNU General Public License
# along with oi-ctd.  If not, see <http://www.gnu.org/licenses/>.

import socket
import lcm
from oi import ctd_t
import struct
import binascii

class WisslConnection():
    
    def __init__(self, host_a, host_b, port):
        self.host_a = host_a
        self.host_b = host_b
        self.port = port
        self.s_a = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        self.s_b = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
        self.s_a.settimeout(2)
        self.s_b.settimeout(2)
        self.host_a_connected = False
        self.host_b_connected = False

    def __enter__(self):
        self.connect_host_a()
        self.connect_host_b()
        return self
    
    def __exit__(self, type, value, traceback):
        self.close()

    def connect_host_a(self):
        if self.host_a_connected is True: return 
        try: 
            self.s_a.connect((self.host_a, self.port))
            self.host_a_connected = True
            print(f'Connected to WiSSL Head A at {self.host_a}:{self.port}')
        except  Exception as e:
            print(f'Error on WISSL Head A connect: {e}')
            self.host_a_connected = False

    def connect_host_b(self):
        if self.host_b_connected is True: return 
        try: 
            self.s_b.connect((self.host_b, self.port))
            self.host_b_connected = True
            print(f'Connected to WiSSL Head B at {self.host_b}:{self.port}')
        except  Exception as e:
            print(f'Error on WISSL Head B connect: {e}')
            self.host_b_connected = False

    def close(self):
        self.s_a.close()
        self.s_b.close()
        

    def update_ctd(self, temp_c:float, salinity_psu:float, pressure_dbar:float):
        '''
            2 byte project code 0x033d
            1 byte version 1
            3 doubles - temp (C), salinity (PSU), pressure (dbar)
        '''
        # pack the message
        message = struct.pack('=HBddd', 0x3D03, 0x01,\
            temp_c, salinity_psu, pressure_dbar)

        #print(len(message),binascii.hexlify(message))
        #print(f'{len(message)}:{message}')
        #b = bytearray(message)
        # connect if not connected
        if self.host_a_connected is False:
            self.connect_host_a()
        if self.host_b_connected is False:
            self.connect_host_b()

        # transmit
        try:
            if self.host_a_connected is True:
                self.s_a.send(message)
        except Exception as e:
            print(f'Error on Head A send(): {e}')
            self.host_a_connected = False
        try:
            if self.host_b_connected is True:
                self.s_b.send(message)
        except Exception as e:
            print(f'Error on Head A send(): {e}')
            self.host_a_connected = False
        

    def handle_lcm(self, channel, data):
        msg = ctd_t.decode(data) 
        self.update_ctd(msg.temperature_degc, msg.salinity_psu, msg.pressure_decibar)


if __name__ == '__main__':

    # ARGUMENTS
    from argparse import ArgumentParser
    a = ArgumentParser()
    a.add_argument('--lcm-url', type=str, default="udpm://239.255.76.67:7667?ttl=1")
    a.add_argument('--lcm-channel', type=str, default="CTD_FASTCAT")
    a.add_argument('--wissl-a', type=str, default="192.168.168.130")
    a.add_argument('--wissl-b', type=str, default="192.168.168.131")
    a.add_argument('--wissl-port', type=int, default=2009)
    args = a.parse_args()

    # 3D @ Depth sockets
    with WisslConnection(args.wissl_a, args.wissl_b, args.wissl_port) as wissl:


        # LCM CONNECTION
        print(f'Creating LCM connection to {args.lcm_url} ...')
        lc = lcm.LCM(args.lcm_url)

        lc.subscribe("CTD_FASTCAT", wissl.handle_lcm)

        try:
            while True:
                lc.handle()
        except KeyboardInterrupt:
            pass