#-------------------------------------------------------------------------------
# Name:         Controller for the volume flow
# Purpose:      Allow for a multiprocessed and asynchronous approach to the control
#               of the volume flow to the reactor
# Author:       Jonathan G
#
# Last Version: 10.08.2021
# Copyright:    (c) Institute of Technical Biocatalysis TUHH
# Licence:      GNU GPL-v3 
#-------------------------------------------------------------------------------
from pumps import SerialPump
from pumps_new_era import pumps_new_era
from Flowmeter import Flowmeter
import asyncio
import numpy as np
import pandas as pd
from multiprocessing import Queue, Process
import time

class Controller:

    def __init__(self,CMQueue,MCQueue,InputManager) -> None:
        '''
        General initialization of the python class.
        '''
        ### Variables for the general performance
        ### like the loop and the Queues
        self.loop = asyncio.get_event_loop()
        self.CMQueue = CMQueue
        self.MCQueue = MCQueue

        ### Variables for the Input Manager
        self.InputManager = InputManager
        self.AdminValues = self.InputManager.getAdminValues()
        self.startValues = self.InputManager.getStartValues()
        self.PumpMode = self.InputManager.getPumpMode()
        self.ControlMode = self.InputManager.getControlMode()

        # TODO: implement generally for everything?
        if self.PumpMode == 1:
            self.nPumps = 1
        elif self.PumpMode == 2:
            self.nPumps = 2
        elif self.PumpMode == 3:
            self.nPumps = 4
        
        ### General boolean values
        self.control = False
        self.stop = False
        self.isSteadyState = False
        self.isSetup = False

        ### General control values
        self.ControlAccuracy = self.AdminValues.ControlAccuracy
        self.DampingFactor = self.AdminValues.ControlDamp
        self.FlowCounter = 0

        ### Values for the flow and temperatures
        self.VolumeFlow = 0
        self.lastFlag = 0
        self.LastFlowRate = 0
        self.LastTemperature = 0
        self.PumpFlowrates = pd.Series(np.nan, index=['S1','S2','S3','S4'])
        self.ListFlowRates = []
        self.ListTemperatures = []
        self.OutputFlowRates = []
        self.OutputTemperatures = []

        ### Start the initialization
        self.initialization()
    
    def initialization(self) -> None:
        '''
        Initialization of the physical components as well as starting
        the overall control loop.
        '''
        ### Run the Initialization for the physical components
        self.loop.run_until_complete(self.DeviceInit())

        ### Run the Main loop
        self.loop.run_until_complete(self.main())

    async def DeviceInit(self) -> None:
        '''
        TODO: add check for all pumps required, add DiameterCheck somewhere

        Specific initialization of the pump and the Flowmeter. Also starts the 
        continoous measurement cycle.
        '''
        ### Init the Flow Meter
        self.flowMeter = Flowmeter(self.AdminValues.SensorCOM)
        await self.flowMeter.initialize()
        await asyncio.sleep(0.5)

        ### Clear the buffer and start the measurement cycle
        await self.flowMeter.clear_buffer()
        await self.flowMeter.start_contiMeasure(self.AdminValues.FlowInterval)
    
    async def main(self) -> None:
        '''
        Main function that starts the control loop and waits for both aspects to finish.
        '''

        self.stop = False

        ### Create the communication and control task
        communication = asyncio.create_task(self.getVolumeFlow())
        control = asyncio.create_task(self.controlRoutine())
        
        ### Await for the completion of the task 
        await asyncio.wait([communication,control])#,steadyCheck])
    
    async def getVolumeFlow(self)-> None:
        '''
        Exremely badly named function that is responsible for the communication with
        the other process.
        '''

        while True:

            if self.MCQueue.empty() is False:

                msg = self.MCQueue.get()

                msgList = msg.split(',')
                header = int(msgList[0])
                tail = msgList[1:]

                ### Switch for different header
                if header == 1: # Start the measurement

                    #print(msg)
                    self.VolumeFlow = float(tail[0])
                    for i in range(1,len(tail)):
                        self.PumpFlowrates['S%i'%(i)] = tail[i]
                    self.ListFlowRates.clear()
                    self.OutputTemperatures.clear()
                    self.OutputFlowRates.clear()
                    self.FlowCounter = 0
                    self.isSteadyState = False
                    self.isSetup = False
                    self.startControl()

                elif header == 2:   # Change volume flow

                    self.VolumeFlow = float(tail[0])
                    #print(tail)
                    for i in range(1,len(tail)):
                        self.PumpFlowrates['S%i'%(i)] = tail[i]
                    self.OutputFlowRates.clear()
                    self.OutputTemperatures.clear()
                    self.ListFlowRates.clear()
                    self.FlowCounter = 0
                    self.isSteadyState = False
                    self.isSetup = False

                elif header == 3: # Stop the control cycle

                    self.stop = True
                    await self.stopControl()
                    await self.shutdown()
                    break
                
                elif header == 4: # Return the flow values

                    if not self.isSteadyState: # Do not return values if the steady state hasn't been reached.

                        self.CMQueue.put('0,0,0,0,0')

                    else:
                        ### Average the flow and temperature values during the 
                        ### measurement cycle
                        avgFlow = str(np.average(self.OutputFlowRates))
                        defFlow = str(np.std(self.OutputFlowRates))
                        avgTemp = str(np.average(self.OutputTemperatures))
                        defTemp = str(np.std(self.OutputTemperatures))
                        
                        ### Create the transfer message
                        msg = ','.join(('1',avgFlow,defFlow,avgTemp,defTemp))
                        self.CMQueue.put(msg)

                        ### Clear the Output lists
                        self.OutputFlowRates.clear()
                        self.OutputTemperatures.clear()
            

            await asyncio.sleep(self.AdminValues.ControlInterval/1000)

    async def controlRoutine(self):
        '''
        TODO: potentially faster with different methods for each mode
        TODO: add propper control if necessary. Implement waiting period
        Main control Routine that actually controls the pump.
        '''
        while not self.stop:

            if self.control:

                flowRates,Temperatures,Flags = await self.flowMeter.get_conti_flow_measurement()

                if len(flowRates)>0:
                    self.LastFlowRate = np.average(flowRates)
                    self.LastTemperature = np.average(Temperatures)
                    #print(self.LastFlowRate)
                    self.ListFlowRates.append(self.LastFlowRate)
                    self.ListTemperatures.append(self.LastTemperature)
                    self.FlowCounter +=1
                    await self.steadyStateCheck()

                    if self.isSteadyState:
                        self.OutputFlowRates.append(self.LastFlowRate)
                        self.OutputTemperatures.append(self.LastTemperature)

                    if len(self.ListFlowRates)>50:
                        self.ListFlowRates[:-3] = []
                        self.ListTemperatures[:-3] = []

                    if self.PumpMode == 1:
                        newPumpVel = self.pump.velocity+self.DampingFactor*(self.VolumeFlow-self.LastFlowRate)

                        if newPumpVel < 0 or newPumpVel> 5500:
                            await self.shutdown()
                            raise Exception("Problem with the setup")

                        await self.pump.setVel(newPumpVel)

                    # TODO: zusammenfassen?
                    elif self.PumpMode == 2:
                        if self.ControlMode == 1:
                            for i in range(2):
                                change = self.DampingFactor*(self.VolumeFlow-self.LastFlowRate/1000)*self.PumpFlowrates['S%i'%(i+1)]/self.VolumeFlow
                                self.pump.changeRate('0%i'%(i+1),change)
                            
                    elif self.PumpMode == 3:
                        if self.ControlMode == 1:
                            for i in range(self.nPumps):
                                change = self.DampingFactor*(self.VolumeFlow-self.LastFlowRate/1000)*self.PumpFlowrates['S%i'%(i+1)]/self.VolumeFlow
                                self.pump.changeRate('0%i'%(i+1),change)
                        
            await asyncio.sleep(self.AdminValues.ControlInterval/1000)

    async def steadyStateCheck(self)-> None:
        '''
        A simple check whether the Control system has reached the steady state.

        IMPORTANT: THIS DOES NOT MEAN THE REACTION IS IN EQUILIBRIUM.
        '''
        
        if len(self.ListFlowRates) >=3:

            if (self.VolumeFlow-np.average(self.ListFlowRates[-3:])) < self.VolumeFlow*self.ControlAccuracy*0.01:
                self.isSteadyState = True

            else:
                self.isSteadyState = False

    def startControl(self)-> None:
        '''
        Helper function to start the control.
        '''
        self.control = True
    
    async def stopControl(self)-> None:
        '''
        Helper function to stop the control.
        '''
        self.control = False
    
    async def shutdown(self):
        '''
        Shutdown routine. Should be called whenever an unexpected error occurs.
        '''
        pass
        '''
        print('Shutdown of Control')
        await self.flowMeter.stop_contiMeasure()
        await self.flowMeter.clear_buffer()
        self.flowMeter.close()
        if self.PumpMode == 1:
            await self.pump.setVel(0)
        elif self.PumpMode == 2:
            for i in range(self.nPumps):
                self.pump.stop('0%i'%(i+1))
        elif self.PumpMode == 3:
            for i in range(self.nPumps):
                self.pump.stop('0%i'%(i+1))
        '''