# pip install
import numpy as np
import os
import sys
import time
import traceback

paths = ["../../src", "../../../guitastro_device_ascomcam/src"]
for path in paths:
    if path not in sys.path:
            sys.path.insert(0,path)

import guitastro

from guitastro_device_ascomcam import Device_Ascomcam

try:
    from pymodbus.client import ModbusTcpClient
    have_modbus = True
except:
    have_modbus = False

if __name__ == "__main__":

    ima = guitastro.Ima()
    path_products = ima.copy_data2products()

    if have_modbus:
        client = ModbusTcpClient(host='172.16.10.10', port=502)
        client.connect()
    else:
        print("No modbus")

    name = "T1M_FinderCam"
    device = "ASCOM.ASICamera2.Camera"
    model = "ZWO"
    serial_number = "12345"
    description = "Test"
    dev = Device_Ascomcam("ASCOMCAM", transport="USB", name=name, description=description, model=model, serial_number=serial_number, device=device)
    # ------------------------
    real = False
    dev.open(real)
    # ------------------------
    cam = dev.components['camera'][1]
    # ------------------------
    cam.ima.fcontext_create("t1m_guide", "Test autoguiding")
    cam.ima.fcontext = "t1m_guide"
    cam.ima.extension = ".fit"
    rootdir0 = cam.ima.rootdir
    cam.ima.rootdir = os.path.join(rootdir0, "t1m")
    os.makedirs(cam.ima.rootdir, exist_ok=True)
    print(f"{cam.__repr__()}")

    # ------------------------
    if real == False:
        import shutil
        fname_in  = os.path.join(cam.ima.rootdir, "..", "..", "data", "m57.fit")
        fname_out = os.path.join(cam.ima.rootdir, "m57.fit")
        shutil.copy(fname_in, fname_out)

    # ------------------------
    for k in range(4):
        try:
            if real:
                # ------------------------
                exptime = 0.001
                cmd = f"camera SET exptime {exptime}"
                print("*"*20,f"\ncmd => \"{cmd}\"")
                dev.commandstring(cmd)
                # ------------------------
                cmd = "camera DO ACQ START"
                print("*"*20,f"\ncmd => \"{cmd}\"")
                dev.commandstring(cmd)
                while True:
                    time.sleep(0.5)
                    # ------------------------
                    cmd = "camera GET timer"
                    timer = dev.commandstring(cmd)
                    if timer == -1:
                        break
            else:
                cam.ima.load(fname_out)
                time.sleep(5)
            # -----------------------
            astrotable = cam.ima.sextractor(kappa_sigma = 3)
            astrotable.useFileNames = True
            astrotable.fn.fcontext_create("t1m_guide")
            astrotable.fn.fcontext = "t1m_guide"
            astrotable.fn.rootdir = cam.ima.rootdir
            fname = astrotable.fn.join("sextractor.txt")
            astrotable.write(fname, format="astrotable", overwrite=True)
            astrotable.keepcols(['x','y','flux', 'flag', 'fwhm'])
            array = np.array([astrotable.getcol('x'),astrotable.getcol('y'),astrotable.getcol('flux'),astrotable.getcol('flag'),astrotable.getcol('fwhm')]).T
            # --- Sort the matrix by the column index 2
            array_sorted = array[np.lexsort(([-1]*array[:,[2]]).T)]
            INDX_X = 0
            INDX_Y = 1
            INDX_FLUX = 2
            INDX_FLAG = 3
            INDX_FWHM = 4
            astrotable.setcol('x', array_sorted[:,INDX_X])
            astrotable.setcol('y', array_sorted[:,INDX_Y])
            astrotable.setcol('flux', array_sorted[:,INDX_FLUX])
            astrotable.setcol('flag', array_sorted[:,INDX_FLAG])
            astrotable.setcol('fwhm', array_sorted[:,INDX_FWHM])
            fname = astrotable.fn.join("sextractor_sorted.txt")
            astrotable.write(fname, format="astrotable", overwrite=True)
            # ---
            naxis1 = cam.ima.getkwd("NAXIS1")
            naxis2 = cam.ima.getkwd("NAXIS2")
            border = 50
            xmin = border
            xmax = naxis1-border
            ymin = border
            ymax = naxis2-border
            ident_limit = 5
            ident_limit2 = ident_limit*ident_limit
            nb_star = 5
            # ---
            if k == 0:
                # --- First image
                nlig, ncol = array_sorted.shape
                valids = np.ones(nlig)
                for klig in range(nlig):
                    if array_sorted[klig, INDX_FLAG] > 0:
                        valids[klig] = 0
                    x = array_sorted[klig, INDX_X]
                    if x < xmin or x > xmax:
                        valids[klig] = 0
                    y = array_sorted[klig, INDX_Y]
                    if y < ymin or y > ymax:
                        valids[klig] = 0
                array_sorted0 = array_sorted.copy()
            else:
                # --- match stars and compute dx, dy
                nlig0, ncol0 = array_sorted0.shape
                nlig, ncol = array_sorted.shape
                decals = []
                kstar = 0
                for klig0 in range(nlig0):
                    if kstar > nb_star:
                        break
                    if valids[klig0] == 0:
                        continue
                    x0 = array_sorted0[klig0, INDX_X]
                    y0 = array_sorted0[klig0, INDX_Y]
                    for klig in range(nlig):
                        x = array_sorted[klig, INDX_X]
                        y = array_sorted[klig, INDX_Y]
                        if not real:
                            x += 0.1*np.random.normal(1)
                            y += 0.1*np.random.normal(1)
                        dx = x - x0
                        dy = y - y0
                        d2 = dx*dx + dy*dy
                        if d2 < ident_limit2:
                            # --- same star
                            decal = [dx, dy, x0, y0]
                            decals.append(decal)
                            continue
                    kstar += 1
                decals = np.array(decals)
                #print(f"{decals=}")
                # --- compute median of dx, dy
                dxs = decals[:,0]
                dys = decals[:,1]
                dx_med = np.median(dxs)
                dy_med = np.median(dys)
                print(f"{dx_med=:.2f} {dy_med=:.2f}")
                # ----------------------------
                if dx_med < 0:
                    m_alpha = 54
                else:
                    m_alpha = 55
                if dy_med < 0:
                    m_delta = 50
                else:
                    m_delta = 51
                try:
                    if have_modbus:
                        client.write_coils(m_alpha, True, unit=0)
                        client.write_coils(m_delta, True, unit=0)
                        time.sleep(0.3)
                        client.write_coils(m_alpha, False, unit=0)
                        client.write_coils(m_delta, False, unit=0)
                except:
                    print("Modbus error")
            # ----------------------------


        except:
            traceback.print_exc(file=sys.stdout)
    # ------------------------
    print("*"*20,"\nClose")
    dev.close()