#!/usr/bin/env python3

import cv2 as cv
from cv2 import aruco
import numpy as np
from time import time

# Tabelle mit Segmentzuständen und korespondierenden Ziffern
digit_tab = { 0x77: '0', 0x12: '1', 0x5d: '2', 0x5b: '3', 0x3a: '4', 0x6b: '5',
              0x6f: '6', 0x72: '7', 0x7f: '8', 0x7b: '9', 0x00:  '', 0x25: 'L'  }

# Bestimmung des numerischen Werts einer Ziffer aus den Segmentzuständen
def segs2num(seg_states):
  bits = int(''.join(map(str, (seg_states))), 2)
  return digit_tab[bits] if bits in digit_tab else '?'

# Kamera initialisieren
cap = cv.VideoCapture(0)
cap.set(cv.CAP_PROP_AUTO_EXPOSURE, 3)
cap.set(cv.CAP_PROP_FRAME_WIDTH, 1280)
cap.set(cv.CAP_PROP_FRAME_HEIGHT, 720)
cap.set(cv.CAP_PROP_BRIGHTNESS, 0)

# Marker-Dictionary und Parameter für den ArUco-Detektor
aruco_dict   = aruco.getPredefinedDictionary(0)
aruco_params = aruco.DetectorParameters()

aruco_params.cornerRefinementMethod    = cv.aruco.CORNER_REFINE_APRILTAG
aruco_params.aprilTagMinClusterPixels  = 400
aruco_params.aprilTagMinWhiteBlackDiff = 50

# ArUco-Detektor anlegen
aruco_detektor = cv.aruco.ArucoDetector(aruco_dict, aruco_params)

# Parameter für ROI und Marker
roi_w, roi_h = 58, 26       # Größe der ROI in mm
mk_x, mk_y   =  1, 27       # Position des Markers relativ zur ROI
mk_w, mk_h   = 57, 16       # Größe des Markers in mm
pxl_per_mm   =  4           # Skalierungsfaktor des entzerrten Bilds in Pixel/mm

mk_corners = np.array([[0, 0], [mk_w, 0], [mk_w, mk_h], [0, mk_h]])  # Eckpunkte des Markers 
mk_corners = np.float32(pxl_per_mm * ([mk_x, mk_y] + mk_corners))    # in Pixel relativ zur ROI

ref_img = cv.imread('refimage.png')  # Referenzbild einlesen
tpl_img = ref_img[:, :, 1]           # Bild für Template-Matching extrahieren (Grünkanal)
seg_img = ref_img[:, :, 2]           # Bild mir Segementreferenzpunkten extrahieren (Rotkanal)

nseg = 4 * 7 + 3 + 1    # Anzahl der Segmente (Ziffern, Dezimalpunkte und Vorzeichen)
seg_pts = np.array([ np.argwhere(seg_img == 200 + i)[0] for i in range(nseg) ])

t0 = None  # Startzeit der Messungen
old_val = None

fp = open('data', 'w')  # Datei für die Messdaten öffnen

while True:
  _, color_img = cap.read()                                       # Kamerabild einlesen
  gray_img = cv.cvtColor(color_img, cv.COLOR_BGR2GRAY)            # in Graubild konvertieren
  det_corners, ids, _ = aruco_detektor.detectMarkers(gray_img)    # Marker detektieren
  aruco.drawDetectedMarkers(color_img, det_corners, ids)  # Ergebnisse im Farbbild markieren

  cv.imshow('color image', color_img)                     # Farbbild anzeigen

  if(det_corners):      # falls ein Marker gefunden wurde,
    # über die 4 Ecken des Markerns die perspektivische Transformation bestimmen
    trans = cv.getPerspectiveTransform(det_corners[0][0], mk_corners)
    # und die ROI perspektivisch entzerren
    roi_img = cv.warpPerspective(gray_img, trans, (pxl_per_mm * roi_w, pxl_per_mm * roi_h))

    # Bildrauschen reduzieren und Bild binarisieren
    roi_img = cv.GaussianBlur(roi_img, (9, 9), 0) 
    bin_img = cv.adaptiveThreshold(roi_img, 1, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY, 11, 3)

    # binarisiertes Bild invertieren, mit Template-Bild per kreuzkorrelieren und damit den Offset
    # des Ziffernblocks innerhalb der ROI bestimmen
    corr_img = cv.matchTemplate(np.float32(1-bin_img), np.float32(tpl_img), cv.TM_CCORR)
    _, _, _, (off_x, off_y) = cv.minMaxLoc(corr_img)

    # Erstellung einer Liste mit den Zuständen der einzelnen Segmente
    segs = []
    for i in range(nseg):
      y, x = seg_pts[i] + (off_y, off_x)   # Koordinaten der Segmentreferenzpunkte
      roi_img[y, x] = 255                  # im ROI-Bild markieren

      # Sind in einer 5x5-Pixel-Umgebung um des Segementpunkt weniger als die Hälfte der Pixel schwarz,
      # wird das Segment als aktiv betrachtet
      segs.append(int(np.sum(bin_img[y-2:y+3, x-2:x+3]) < 13))

    # Aus den Segmentzuständen den angezeigten Zahlenwert ermitteln
    val = segs2num(segs[0:7]) + segs2num(segs[8:15]) + segs2num(segs[16:23]) + segs2num(segs[24:31])

    # Sind alle Ziffern eindeutig bestimmt, wird aus der Position des Dezimalpunkts
    # der Messbereich bestimmt und den Wert entsprechend skaliert:
    try:
      val = float(val)
    except ValueError:
      pass
    else:
      if segs[7]:
        val /= 1e3
      elif segs[15]: 
        val /= 1e2
      elif segs[23]:
        val /= 1e4
      else:
        val /= 1e3
      if segs[31]:   # Vorzeichen berücksichtigen
        val = -val

      # zur Unterdrückung von Glitches beim Umschalten der LCD-Segmente
      # Messwert nur dann ausgeben, wenn er mit dem letzten übereinstimmt
      if val == old_val:
        t = time()
        if t0 == None:
          t0 = t
        out = '%10.3f %8.4f' % (t - t0, val)  # Zeit und Messwert ausgeben
        print(out)
        print(out, file=fp)
      old_val = val

    cv.imshow('ROI image', roi_img)
    cv.imshow('binarized image', 255 * bin_img)

  if cv.waitKey(1) == ord('q'):  # Tastenabfrage zum Beenden der Schleife
    break

fp.close()