# 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
import time
import random

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

    def __enter__(self):
        self.connect_host_a()
        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 at {self.host_a}:{self.port}')
        except  Exception as e:
            print(f'Error on WISSL Head connect: {e}')
            self.host_a_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('=HBdddBBBBBBB', 0x3D03, 0x01,\
            temp_c, salinity_psu, pressure_dbar,0,0,0,0,0,0,0)

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

        # transmit
        try:
            if self.host_a_connected is True:
                self.s_a.send(message)
                print(f'Sent CTD of {temp_c}C, {salinity_psu}PSU, {pressure_dbar}dbar')
        except Exception as e:
            print(f'Error on Head A send(): {e}')
            self.host_a_connected = False
        


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.1.87")
    a.add_argument('--wissl-port', type=int, default=2003)
    args = a.parse_args()

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


        try:
            while True:
                rand = random.randint(0, 100)/100.0
                wissl.update_ctd(3.38+rand, 5.9+rand, 560.111+rand)
                time.sleep(.5)
        except KeyboardInterrupt:
            pass