from util import pos, clamp, cast_to_cm
import parameters
from time import time, sleep
import zmq
import syslab
import logging

# Ensure that we see "info" statements below
logging.basicConfig(level=logging.INFO)

# # Parameters
Kp = 0.4  # P factor for our controller.
Ki = 0.8  # I factor for our controller.

# # Variables
# Target that we are trying to reach at the grid connection.
pcc_target = 0.0

# Controller variables
x_load = 1  # Default splitting factor

# Info on battery state
mobileload_setpoint = 0.0  # Default setpoint
battery_soc = 0.5  # Default SOC (= state of charge)

### Communication
# Handle the sockets we need
# Make a context that we use to set up sockets
context = zmq.Context()

# Set up a socket to subscribe to our splitting factor
splitting_in_socket = context.socket(zmq.SUB)
splitting_in_socket.connect(f"tcp://{parameters.SUPERVISOR_IP}:{parameters.SUPERVISOR_PORT}")

# Ensure we only see message on the load's splitting factor
splitting_in_socket.subscribe(parameters.TOPIC_LOAD_SPLITTING)

### Unit connections
# TODO step 1.2: Set up connection to control the battery and reconstruct the pcc (remember that vswitchboard is still not working)
gaia = syslab.WindTurbine("vgaia1")
dumpload = syslab.Dumpload("vload1")
mobload1 = syslab.Dumpload("vmobload1")
pv319 = syslab.Photovoltaics("vpv319")
batt = syslab.Battery('vbatt1')

### Import your controller class
# TODO step 1.2: Import the controller class from "simlab_controller_d5_load.py" or copy/paste it here and pick reasonable controller parameters
# Note: The controller is identical to Day 5 with the exception of incorporating the splitting factor
from simlab_controller_d5_load import PIDController

pid = PIDController(Kp=Kp, Ki=Ki, Kd=0.0,
                    u_min=parameters.MIN_LOAD_P,
                    u_max=parameters.MAX_LOAD_P,
                    Iterm=0.0)


# Ensure the mobile load is on before we start (The sleeps wait for the load to respond.)
while not mobload1.isLoadOn().value:
    print("Starting load")
    mobload1.startLoad()
    sleep(2)


try:
    while True:
        try:
            # Try to connect the the supervisor.
            # If we have a connection to the supervisor, get our requested splitting factor.
            # If none have come in, continue with previous splitting factor.
            # We put this in a while-loop to ensure we empty the queue each time.
            while True:
                # Receive the latest splitting factor
                incoming_str = splitting_in_socket.recv_string(flags=zmq.NOBLOCK)
                # The incoming string will look like "load_split;0.781",
                # so we split it up and take the last part.
                x_load = float(incoming_str.split(" ")[-1])
                logging.info(f"New splitting factor: {x_load}")
        except zmq.Again as e:
            # No (more) new messages, move along
            pass

        # Poll the grid connection to get the current grid exchange.
        pcc_p = cast_to_cm(-(
                gaia.getActivePower().value 
              + batt.getActivePower().value 
              + pv319.getACActivePower().value
              - dumpload.getActivePower().value 
              - mobload1.getActivePower().value 
        ))

        # Calculate new requests using PID controller
        mobileload_setpoint = pid.update(pcc_p.value, x_load=x_load)

        # Ensure we don't exceed our bounds for the load
        mobileload_setpoint = clamp(parameters.MIN_LOAD_P, mobileload_setpoint, parameters.MAX_LOAD_P)

        # Send the new setpoint to the load
        # TODO step 1.2: Send the new setpoint to the load
        mobload1.setPowerSetPoint(mobileload_setpoint)
        logging.info(f"Sent setpoint: {mobileload_setpoint}")

        # Loop once more in a second
        sleep(1)
finally:
    # Clean up by closing our socket.
    # TODO step 1.2: Set the setpoint of the mobile load to zero and shut it down after use
    mobload1.setPowerSetPoint(0.)
    mobload1.stopLoad()
    splitting_in_socket.close()