'''
Created on Tue Jan 9 2018

@author: jan
'''

import time
import globalvars
import queue
import zmq

class PID:
    def __init__(self, dev_num_adc, channel_num_adc, dev_num_dac, channel_num_dac, pid_num, kp=None, ki=None, kd=None, setpoint=None):
        if kp is None:
          self.kp = 1
        else:
          self.kp = float(kp)

        if ki is None:
          self.ki = 1
        else:
          self.ki = float(ki)

        if kd is None:
          self.kd = 1
        else:
          self.kd = float(kd)

        if setpoint is None:
          self.setpoint = 0.0
        else:
          self.setpoint = float(setpoint)

        self.dev_num_adc = dev_num_adc
        self.channel_num_adc = channel_num_adc
        self.dev_num_dac = dev_num_dac
        self.channel_num_dac = channel_num_dac
        self.pid_num = pid_num

        self.ITerm     = 0
        self.lastInput = self.setpoint
        self.outMin    = 0
        self.outMax    = 5
        self.error     = 0

        self.start = time.time()
        self.stop  = 0

        # Order of initialisation shouldn't matter, because zmq handel everything
        self.context = zmq.Context(1)
        self.SPIclient = self.context.socket(zmq.REQ)
        self.SPIclient.connect("tcp://localhost:5555")

    def computePID(self):
        #Get ADC value
        #self.out_queue.put(['ADC', self.dev_num, self.channel_num, self.pid_num])
        #Input = self.in_queue.get()
        msg = ['ADC', self.dev_num_adc, self.channel_num_adc, self.pid_num]
        self.SPIclient.send('\t'.join(str(x) for x in msg).encode())
        Input = float(self.SPIclient.recv().decode())

        #Get elapsed time for right PID constants
        self.stop = time.time()
        elapsed_time = self.stop - self.start
        self.start = time.time()

        #print(type(self.ki))
        Ki = float(self.ki) * elapsed_time
        Kd = float(self.kd) / elapsed_time

        #Calculate working variables
        self.error = float(self.setpoint) - Input
        dInput     = Input - self.lastInput
        self.ITerm = self.ITerm + (Ki * self.error)
        # Check min-max for I-Term
        if self.ITerm > self.outMax:
          self.ITerm = self.outMax
        elif self.ITerm < self.outMin:
          self.ITerm = self.outMin

        # Calculate PID Output
        self.Output = float(self.kp) * self.error + self.ITerm - Kd * dInput
        # Check min-max for output
        if self.Output > self.outMax:
          self.Output = self.outMax
        elif self.Output < self.outMin:
          self.Output = self.outMin

        # Save variable for next run
        self.lastInput  = Input

        # Set output value
        #self.out_queue.put(['DAC', self.dev_num, self.channel_num, self.pid_num, self.Output])
        msg = ['DAC', self.dev_num_dac, self.channel_num_dac, self.pid_num, self.Output]
        self.SPIclient.send('\t'.join(str(x) for x in msg).encode())
        ans = self.SPIclient.recv().decode()
        #if self.in_queue.get() != 'ACK':
        if ans != 'ACK':
            print('Error setting DAC-value')

    def setSetpoint(self, setpoint):
        #self.ITerm = 0
        self.setpoint = setpoint

    def setKp(self,P):
        self.kp=P

    def setKi(self,I):
        self.ki=I

    def setKd(self,D):
        self.kd=D

    def setOutmax(self,outMax):
        self.outMax = outMax
        
    def setOutmin(self,outMin):
        self.outMin = outMin

    def getSetpoint(self):
        return self.setpoint

    def getError(self):
        return self.error

    def getKp(self):
        return self.kp

    def getKi(self):
        return self.ki

    def getKd(self):
        return self.kd

    def getOutput(self):
        return self.Output

    def getTemperatur(self):
        return self.lastInput

    def getRunValues(self):
        return [self.lastInput, self.error, self.Output]


def run(pid_obj, freq, num):
    context = zmq.Context()
    server = context.socket(zmq.REP)
    port = 5556 + num
    server.bind("tcp://*:%d" % port)
    poller = zmq.Poller()
    poller.register(server, zmq.POLLIN)

    stop = False

    while not globalvars.stopall and not stop:
        cmd_recv = ''
        if globalvars.pidrun[num]:
            pid_obj.computePID()
            #try:
            #    msg = in_queue.get(True,1/freq)
            #except queue.Empty:
            #    msg = ['None']
            socks = dict(poller.poll(1000))
            if server in socks and socks[server] == zmq.POLLIN:
                cmd_recv = server.recv_string()
            else:
                cmd_recv = 'None'
        else:
            socks = dict(poller.poll(1000))
            if server in socks and socks[server] == zmq.POLLIN:
                cmd_recv = server.recv_string()

        msg = cmd_recv.split('\t')
        ans = 'ACK'
        if   msg[0] == 'None':
            pass
        elif msg[0] == 'set_setpoint':
            pid_obj.setSetpoint(msg[1])
            server.send_string('ACK')
        elif msg[0] == 'set_kp':
            pid_obj.setKp(msg[1])
            server.send_string('ACK')
        elif msg[0] == 'set_ki':
            pid_obj.setKi(msg[1])
            server.send_string('ACK')
        elif msg[0] == 'set_kd':
            pid_obj.setKd(msg[1])
            server.send_string('ACK')
        elif msg[0] == 'set_outmax':
            pid_obj.setOutmax(float(msg[1]))
            server.send_string('ACK')
        elif msg[0] == 'set_outmin':
            pid_obj.setOutmin(float(msg[1]))
            server.send_string('ACK')
        elif msg[0] == 'get_setpoint':
            #out_queue.put(pid_obj.getSetpoint() )
            server.send_string(str(pid_obj.getSetpoint()))
        elif msg[0] == 'get_error':
            #out_queue.put(pid_obj.getError() )
            server.send_string(str(pid_obj.getError()))
        elif msg[0] == 'get_kp':
            #out_queue.put(pid_obj.getKp() )
            server.send_string(str(pid_obj.getKp()))
        elif msg[0] == 'get_ki':
            #out_queue.put(pid_obj.getKi() )
            server.send_string(str(pid_obj.getKi()))
        elif msg[0] == 'get_kd':
            #out_queue.put(pid_obj.getKd() )
            server.send_string(str(pid_obj.getKd()))
        elif msg[0] == 'lock':
            print('Starting PID')
            server.send_string('Starting PID')
        elif msg[0] == 'get_output':
            #out_queue.put(pid_obj.getOutput() )
            server.send_string(str(pid_obj.getOutput()))
        elif msg[0] == 'get_temperatur':
            #out_queue.put(pid_obj.getTemperatur() )
            server.send_string(str(pid_obj.getTemperatur()))
        elif msg[0] == 'get_run_values':
            #out_queue.put(pid_obj.getRunValues() )
            server.send_string(str(pid_obj.getRunValues()))

        elif msg[0] == 'END':
            server.send_string('ACK')
            stop = True