from util import pos, clamp, soc_scaler, cast_to_cm
import parameters
from time import time, sleep
import zmq
import logging
import syslab
logging.basicConfig(level=logging.INFO)

# Controller 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_battery = 1  # Default splitting factor

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

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

# Set up a socket we can use to publish our soc on
soc_out_socket = context.socket(zmq.PUB)
soc_out_socket.bind(f"tcp://*:{parameters.BATTERY_SOC_PORT}")

# 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 battery's splitting factor
splitting_in_socket.subscribe(parameters.TOPIC_BATTERY_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_batt.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_batt import PIDController

pid = PIDController(Kp=Kp, Ki=Ki, Kd=0.0,
                    u_min=parameters.MIN_BATTERY_P,
                    u_max=parameters.MAX_BATTERY_P,
                    Iterm=0.0)


# Put everything in a "try" block so clean-up is easy
try:
    while True:
        try:
            # Try to connect to 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,
            # so we always have the latest value.
            while True:
                # Receive the latest splitting factor
                incoming_str = splitting_in_socket.recv_string(flags=zmq.NOBLOCK)
                # The incoming string will look like "batt_split;0.781",
                # so we split it up and take the last bit forward.
                x_battery = float(incoming_str.split(" ")[-1])
                logging.info(f"New splitting factor: {x_battery}")
        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 
        ))
        # Check our own state of charge
        battery_soc = batt.getSOC()  # TODO step 1.2: Read the true battery SOC

        # Calculate new requests using PID controller
        battery_setpoint = pid.update(pcc_p.value, x_batt=x_battery)

        # Ensure we don't exceed our bounds for the battery
        battery_setpoint = clamp(parameters.MIN_BATTERY_P, battery_setpoint, parameters.MAX_BATTERY_P)

        # Send the new setpoint to the battery
        # TODO step 1.2: Send the new setpoint to the battery
        batt.setActivePower(battery_setpoint)
        logging.info(f"Sent setpoint: {battery_setpoint}")

        # Publish our current state of charge for the supervisory controller to see
        soc_out_socket.send_string(f"{parameters.TOPIC_BATTERY_SOC} {battery_soc:.06f}")

        # Loop once more in a second
        sleep(1)
finally:
    # Clean up by closing our sockets.
    # TODO step 1.2: Set the setpoint of the battery to zero after use
    batt.setActivePower(0.)
    splitting_in_socket.close()
    soc_out_socket.close()