#!/bin/python3
import time
import websockets
import asyncio
import io

import cv2
from picamera2 import Picamera2

from utils import writeInFormat, readInFormat

apitoken = "secret_token 321"
deviceName = "demo001"
url = "wss://savox.protopaja.aalto.fi/api/websockets"
headers = {
  "apitoken": apitoken,
  "devicename": deviceName,
  "type": "device"
}

#2592x1944
w_hd = 640 #2592 #640
h_hd = 480 #1944 #480

#256x384
w_thermal = 256
h_thermal = 192 #192, 384
pictureDelay = 0.0

try:
  picam = Picamera2()
  config = picam.create_preview_configuration(main={"size": (w_hd, h_hd)})
  picam.configure(config)
  picam.start()
except:
  print("Error opening picamera")

try:
  cam = cv2.VideoCapture(-1)
  cam.set(cv2.CAP_PROP_FRAME_WIDTH, w_thermal)
  cam.set(cv2.CAP_PROP_FRAME_HEIGHT, h_thermal)
  cam.set(cv2.CAP_PROP_FPS, 25)
except:
  print("Error opening thermal camera")

if not cam.isOpened():
  exit()

def readCameraHDBuffer():
  data = io.BytesIO()
  picam.capture_file(data, format="jpeg")
  return data.getbuffer().tobytes()

def readCameraThermalBuffer():
  _, frame = cam.read()
  _, buf = cv2.imencode(".jpg", frame)
  return buf.tobytes()

async def sendData():
  while True:
    try:
      async with websockets.connect(url, extra_headers = headers) as ws:
        print("Connected to server, sending data...")
        while True:

          therm_data = readCameraThermalBuffer()
          hd_data = readCameraHDBuffer()

          data = bytearray()
          data += writeInFormat("devicename", bytes(deviceName, "utf-8"))
          data += writeInFormat("therm_data", therm_data)
          data += writeInFormat("hd_data", hd_data)
          data += writeInFormat("therm_w", w_thermal.to_bytes(4, 'big'))
          data += writeInFormat("therm_h", h_thermal.to_bytes(4, 'big'))
          data += writeInFormat("hd_w", w_hd.to_bytes(4, 'big'))
          data += writeInFormat("hd_h", h_hd.to_bytes(4, 'big'))

          try:
            await ws.send(data)
          except:
            print("Could not send data")
          await ws.recv()

          time.sleep(pictureDelay)
    except websockets.ConnectionClosed as e:
      print("Connection closed, trying to reconnect...")
      continue

if __name__ == '__main__':
  while True:
    try:
      loop = asyncio.get_event_loop()
      loop.run_until_complete(sendData())
      loop.run_forever()
    except KeyboardInterrupt:
      break
    except Exception as e:
      print("Exception:", e)
      print("Attempting to reconnect in 1 second.")
      time.sleep(1)
  cam.release() # never gets called, fix later

