from PyQt5 import QtWidgets
from PyQt5.QtCore import QThread
from scipy.ndimage import center_of_mass
import numpy as np
from pymodaq.daq_utils.daq_utils import linspace_step
from pymodaq_pid.utils import PIDModelGeneric, OutputToActuator


class PIDModelBeamSteering(PIDModelGeneric):
    limits = dict(max=dict(state=True, value=1),
                  min=dict(state=True, value=-1),)
    konstants = dict(kp=4, ki=0.05, kd=0.001)

    Nsetpoint = 2
    setpoint_ini = [600. for ind in range(Nsetpoint)]

    actuators_name = ["Haxis", "Vaxis"]
    detectors_name = ['Camera']
    params = [
        {'title': 'Threshold', 'name': 'threshold', 'type': 'float', 'value': 10.},
        {'title': 'Correction sign:', 'name': 'correction_sign', 'type': 'int', 'value': 1, 'min': -1, 'max': 1,
         'step': 2},
        {'title': 'Calibration', 'name': 'calibration', 'type': 'group', 'expanded': True, 'visible': True,
         'children': [
            {'title': 'Start calibration:', 'name': 'start_calibration', 'type': 'bool', 'value': False},
            {'title': 'Timeout:', 'name': 'timeout', 'type': 'int', 'value': 10000},
            {'title': 'Calibration scan', 'name': 'calibration_scan', 'type': 'group', 'expanded': True,
             'visible': True, 'children': [
                {'title': 'Number of steps:', 'name': 'number_of_steps', 'type': 'int', 'value': 5},
                {'title': 'Step size:', 'name': 'step_size', 'type': 'int', 'value': 2}]},
            {'title': 'Fit parameters', 'name': 'fit_parameters', 'type': 'group', 'expanded': True, 'visible': True,
             'children': [
                {'title': 'Slope:', 'name': 'slope', 'type': 'float', 'value': 0.0},
                {'title': 'Offset:', 'name': 'offset', 'type': 'float', 'value': 0.0}]}]}
    ]

    def __init__(self, pid_controller):
        super().__init__(pid_controller)
        self.pid_controller = pid_controller

    def update_settings(self, param):
        """
        Get a parameter instance whose value has been modified by a user on the UI
        Parameters
        ----------
        param: (Parameter) instance of Parameter object
        """
        if param.name() == '':
            pass

    def get_beam_position(self, image):
        """
        Get the center of mass of the beam from the image of the camera.
        All the pixels that are under the threshold are nulled so they do not contribute to the center of mass
        calculation.
        Parameters
        ----------
        image: (numpy 2d array) Image from the camera.

        Returns
        -------
        float: the pixel number corresponding to the center of mass of the beam along this axis.
        """
        image = image - self.settings.child('threshold').value()
        image[image < 0] = 0
        x, y = center_of_mass(image)

        return x, y

    def ini_model(self):
        super().ini_model()

    def convert_input(self, measurements):
        """
        Convert the image of the camera into x and y positions of the center of the beam.
        Parameters
        ----------
        measurements: (Ordereddict) Data from the camera

        Returns
        -------
        tuple: the coordinate of the center of the beam
        """
        image = measurements['Camera']['data2D']['Camera_Mock2DPID_CH000']['data']
        x, y = self.get_beam_position(image)
        self.curr_input = [x, y]
        return self.get_beam_position(image)

    def convert_output(self, output, dt, stab=True):
        """
        Convert the output of the PID in units to be fed into the actuator
        Parameters
        ----------
        output: (float) output value from the PID from which the model extract a value of the same units as the actuator

        Returns
        -------
        list: the converted output as a list (if there are a few actuators)

        """
        #print('output converted')
        
        self.curr_output = output
        return OutputToActuator(mode='rel', values=[output])

def main():
    from pymodaq.dashboard import DashBoard
    from pymodaq.daq_utils.daq_utils import get_set_preset_path
    from pymodaq.daq_utils import gui_utils as gutils
    from pathlib import Path
    from PyQt5 import QtWidgets
    from pymodaq_pid.pid_controller import DAQ_PID

    import sys
    app = QtWidgets.QApplication(sys.argv)
    win = QtWidgets.QMainWindow()
    area = gutils.DockArea()
    win.setCentralWidget(area)
    win.resize(1000, 500)
    win.setWindowTitle('PyMoDAQ Dashboard')

    dashboard = DashBoard(area)
    file = Path(get_set_preset_path()).joinpath("BeamSteering.xml")
    if file.exists():
        dashboard.set_preset_mode(file)
        # prog.load_scan_module()
        pid_area = gutils.DockArea()
        pid_window = QtWidgets.QMainWindow()
        pid_window.setCentralWidget(pid_area)

        prog = DAQ_PID(pid_area, dashboard.modules_manager)
        pid_window.show()
        pid_window.setWindowTitle('PidController')
        QtWidgets.QApplication.processEvents()


    else:
        msgBox = QtWidgets.QMessageBox()
        msgBox.setText(f"The default file specified in the configuration file does not exists!\n"
                       f"{file}\n"
                       f"Impossible to load the DAQ_PID Module")
        msgBox.setStandardButtons(msgBox.Ok)
        ret = msgBox.exec()

    sys.exit(app.exec_())


if __name__ == '__main__':
    main()


