#!/usr/bin/python3
# This file was automatically generated from drawio2cbd with the command:
#   ..\..\..\..\..\..\git\DrawioConvert\__main__.py -Sargvs -E delta=0.2 -F CBD .\AGV.drawio

from CBD.Core import *
from CBD.lib.std import *
from ev3 import _PI as PI
from ev3 import *
from env import PathBlock
from CBD.lib.endpoints import *
from CBD.lib.network import TCPServerSocket, ServerReceiver

DELTA_T = .2

class Merger(BaseBlock):
    def __init__(self, block_name, count=2):
        BaseBlock.__init__(self, block_name, ["IN%d" % (x+1) for x in range(count)], ["OUT1"])
        self.count = count

    def compute(self, curIteration):
        values = []
        for i in range(self.count):
            values.append(self.getInputSignal(curIteration, "IN%d" % (i+1)).value)
        self.appendToSignal(",".join(values))


class Splitter(BaseBlock):
    def __init__(self, block_name, count=2, type=int):
        BaseBlock.__init__(self, block_name, ["IN1"], ["OUT%d" % (x+1) for x in range(count)])
        self.count = count
        self.type = type

    def compute(self, curIteration):
        value = self.getInputSignal(curIteration, "IN1").value
        # print(">>>", value)
        value = value.split(',')
        if len(value) != self.count:
            value = [0.0] * self.count
        for i in range(self.count):
            self.appendToSignal(self.type(value[i]), "OUT%d" % (i+1))


class IntegratorBlock(BaseBlock):
    def __init__(self, block_name):
        BaseBlock.__init__(self, block_name, ["IC", "IN1", "delta_t"], ["OUT1"])
        self.__aoc = 0.0

    def getDependencies(self, curIteration):
        if curIteration == 0:
            return [self._linksIn["IC"].block, self._linksIn["delta_t"].block]
        return [self._linksIn["delta_t"].block]

    def compute(self, curIteration):
        if curIteration == 0:
            value = self.getInputSignal(curIteration, "IC").value
        else:
            time = self.getInputSignal(curIteration, "delta_t").value
            prev = self.getInputSignal(curIteration - 1, "IN1").value
            value = prev * time
        self.__aoc += value
        self.appendToSignal(self.__aoc)


class DerivatorBlock(BaseBlock):
    def __init__(self, block_name):
        BaseBlock.__init__(self, block_name, ["IC", "IN1", "delta_t"], ["OUT1"])
        
    def compute(self, curIteration):
        if curIteration == 0:
            value = self.getInputSignal(curIteration, "IC").value
        else:
            a = self.getInputSignal(curIteration - 1, "IN1").value
            b = self.getInputSignal(curIteration, "IN1").value
            time = self.getInputSignal(curIteration, "delta_t").value
            value = (b - a) / time
        self.appendToSignal(value)


class OdometryBase(CBD):
    def __init__(self, block_name, r=(0.03), d=(0.18)):
        super().__init__(block_name, input_ports=['phiLdot', 'heading', 'phiRdot'], output_ports=['ydot', 'xdot'])

        # Create the Blocks
        self.addBlock(ConstantBlock("r", value=(r)))
        self.addBlock(GenericBlock("sin", block_operator=("sin")))
        self.addBlock(GenericBlock("cos", block_operator=("cos")))
        self.addBlock(AdderBlock("B-OUZVyqj9DiQNmVnVL9-16"))
        self.addBlock(ConstantBlock("half", value=(0.5)))
        self.addBlock(ProductBlock("div2"))
        self.addBlock(ProductBlock("mulX"))
        self.addBlock(ProductBlock("mulY"))
        self.addBlock(ProductBlock("mulSin"))
        self.addBlock(ProductBlock("mulCos"))

        # Create the Connections
        self.addConnection("phiLdot", "B-OUZVyqj9DiQNmVnVL9-16", input_port_name='IN1')
        self.addConnection("phiRdot", "B-OUZVyqj9DiQNmVnVL9-16", input_port_name='IN2')
        self.addConnection("half", "div2", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("B-OUZVyqj9DiQNmVnVL9-16", "div2", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("mulX", "ydot", output_port_name='OUT1')
        self.addConnection("div2", "mulX", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("div2", "mulY", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("mulY", "xdot", output_port_name='OUT1')
        self.addConnection("heading", "sin", input_port_name='IN1')
        self.addConnection("heading", "cos", input_port_name='IN1')
        self.addConnection("mulSin", "mulX", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("sin", "mulSin", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("r", "mulSin", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("r", "mulCos", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("mulCos", "mulY", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("cos", "mulCos", output_port_name='OUT1', input_port_name='IN2')


class Odometry(CBD):
    def __init__(self, block_name, initial=(0,0,0), wheel_radius=(0.03), wheel_axis=(0.18)):
        super().__init__(block_name, input_ports=['phiLdot', 'phiRdot'], output_ports=['x', 'heading', 'y'])

        # Create the Blocks
        self.addBlock(IntegratorBlock("int_w"))
        self.addBlock(ConstantBlock("delta", value=(DELTA_T)))
        self.addBlock(OdometryBase("base", r=(wheel_radius)))
        self.addBlock(IntegratorBlock("int_x"))
        self.addBlock(IntegratorBlock("int_y"))
        self.addBlock(ConstantBlock("init_x", value=(initial[0])))
        self.addBlock(ConstantBlock("init_y", value=(initial[1])))
        self.addBlock(OdometryW("OCegTr4naEkMIVMNTQIZ-73", r=(wheel_radius), d=(wheel_axis)))
        self.addBlock(ConstantBlock("init_w", value=(initial[2])))

        # Create the Connections
        self.addConnection("delta", "int_w", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("delta", "int_y", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("delta", "int_x", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("int_w", "heading", output_port_name='OUT1')
        self.addConnection("int_w", "base", output_port_name='OUT1', input_port_name='heading')
        self.addConnection("phiLdot", "base", input_port_name='phiLdot')
        self.addConnection("phiLdot", "OCegTr4naEkMIVMNTQIZ-73", input_port_name='phiLdot')
        self.addConnection("phiRdot", "base", input_port_name='phiRdot')
        self.addConnection("phiRdot", "OCegTr4naEkMIVMNTQIZ-73", input_port_name='phiRdot')
        self.addConnection("base", "int_x", output_port_name='xdot', input_port_name='IN1')
        self.addConnection("base", "int_y", output_port_name='ydot', input_port_name='IN1')
        self.addConnection("int_x", "x", output_port_name='OUT1')
        self.addConnection("int_y", "y", output_port_name='OUT1')
        self.addConnection("OCegTr4naEkMIVMNTQIZ-73", "int_w", output_port_name='wdot', input_port_name='IN1')
        self.addConnection("init_w", "int_w", output_port_name='OUT1', input_port_name='IC')
        self.addConnection("init_y", "int_y", output_port_name='OUT1', input_port_name='IC')
        self.addConnection("init_x", "int_x", output_port_name='OUT1', input_port_name='IC')


class OdometryW(CBD):
    def __init__(self, block_name, d=(0.18), r=(0.03)):
        super().__init__(block_name, input_ports=['phiLdot', 'phiRdot'], output_ports=['wdot'])

        # Create the Blocks
        self.addBlock(ConstantBlock("rneg", value=(-r)))
        self.addBlock(NegatorBlock("OCegTr4naEkMIVMNTQIZ-39"))
        self.addBlock(AdderBlock("OCegTr4naEkMIVMNTQIZ-43"))
        self.addBlock(ProductBlock("multr"))
        self.addBlock(ConstantBlock("invd", value=(1./d)))
        self.addBlock(ProductBlock("multd"))

        # Create the Connections
        self.addConnection("phiRdot", "OCegTr4naEkMIVMNTQIZ-39", input_port_name='IN1')
        self.addConnection("phiLdot", "OCegTr4naEkMIVMNTQIZ-43", input_port_name='IN1')
        self.addConnection("OCegTr4naEkMIVMNTQIZ-39", "OCegTr4naEkMIVMNTQIZ-43", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("OCegTr4naEkMIVMNTQIZ-43", "multr", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("rneg", "multr", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("invd", "multd", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("multr", "multd", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("multd", "wdot", output_port_name='OUT1')


class ControllerPID(CBD):
    def __init__(self, block_name, T=(50), Kp=(0), Ki=(0), Kd=(0), v=(0.1)):
        super().__init__(block_name, input_ports=['color'], output_ports=['heading', 'velocity'])

        # Create the Blocks
        self.addBlock(ConstantBlock("v", value=(v)))    # 0.67
        self.addBlock(ConstantBlock("threshold", value=(-T)))
        self.addBlock(ConstantBlock("friction", value=(1)))  # 0.77  -- the higher, the sharper
        self.addBlock(AdderBlock("sum"))
        self.addBlock(ProductBlock("mult"))
        self.addBlock(PID("PID", Kp=(Kp), Ki=(Ki), Kd=(Kd)))

        # Create the Connections
        self.addConnection("color", "sum", input_port_name='IN1')
        self.addConnection("threshold", "sum", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("sum", "PID", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("PID", "mult", input_port_name='IN1', output_port_name='OUT1')
        self.addConnection("friction", "mult", input_port_name='IN2', output_port_name='OUT1')
        self.addConnection("mult", "heading", output_port_name='OUT1')
        self.addConnection("v", "velocity", output_port_name='OUT1')


class Controller(CBD):
    def __init__(self, block_name, T=(50), v=(0.1), gamma=(0.0)):
        super().__init__(block_name, input_ports=['color'], output_ports=['heading', 'velocity'])

        # Create the Blocks
        self.addBlock(ConstantBlock("v", value=(v)))
        self.addBlock(ConstantBlock("threshold", value=(-T)))
        self.addBlock(AdderBlock("sum"))
        self.addBlock(ProductBlock("w1qmmAkClArfh_rfX7cO-38"))
        self.addBlock(ConstantBlock("gamma", value=(gamma)))

        # Create the Connections
        self.addConnection("v", "velocity", output_port_name='OUT1')
        self.addConnection("color", "sum", input_port_name='IN1')
        self.addConnection("threshold", "sum", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("sum", "w1qmmAkClArfh_rfX7cO-38", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("gamma", "w1qmmAkClArfh_rfX7cO-38", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("w1qmmAkClArfh_rfX7cO-38", "heading", output_port_name='OUT1')


class PID(CBD):
    def __init__(self, block_name, Kp=(0.03), Ki=(0.0), Kd=(0.05)):
        super().__init__(block_name, input_ports=['IN1'], output_ports=['OUT1'])

        # Create the Blocks
        self.addBlock(ConstantBlock("Kp", value=(Kp)))
        self.addBlock(ConstantBlock("Ki", value=(Ki)))
        self.addBlock(ConstantBlock("Kd", value=(Kd)))
        self.addBlock(IntegratorBlock("int"))
        self.addBlock(DerivatorBlock("deriv"))
        self.addBlock(ConstantBlock("delta_t", value=(DELTA_T)))
        self.addBlock(ProductBlock("prod"))
        self.addBlock(ProductBlock("imult"))
        self.addBlock(ProductBlock("dmult"))
        self.addBlock(ConstantBlock("zero", value=(0)))
        self.addBlock(AdderBlock("sum1"))
        self.addBlock(AdderBlock("sum2"))

        # Create the Connections
        self.addConnection("Kp", "prod", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("IN1", "prod", input_port_name='IN2')
        self.addConnection("IN1", "int", input_port_name='IN1')
        self.addConnection("IN1", "deriv", input_port_name='IN1')
        self.addConnection("delta_t", "deriv", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("delta_t", "int", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("int", "imult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("Ki", "imult", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("deriv", "dmult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("Kd", "dmult", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("zero", "int", output_port_name='OUT1', input_port_name='IC')
        self.addConnection("zero", "deriv", output_port_name='OUT1', input_port_name='IC')
        self.addConnection("prod", "sum1", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("imult", "sum1", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("sum1", "sum2", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("dmult", "sum2", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("sum2", "OUT1", output_port_name='OUT1')


class DifferentialDrive(CBD):
    def __init__(self, block_name, wheel_axis=(0.18), wheel_radius=(0.03), max_wheel_speed=(800*PI/180), p=(0.85)):
        super().__init__(block_name, input_ports=['velocity', 'steering'], output_ports=['phiLdot', 'phiRdot'])

        # Create the Blocks
        self.addBlock(ProductBlock("steeringMult"))
        self.addBlock(ConstantBlock("WheelAxis", value=(wheel_axis)))
        self.addBlock(ConstantBlock("half", value=(0.5)))
        self.addBlock(ProductBlock("halfSteeringMult"))
        self.addBlock(AdderBlock("leftSum"))
        self.addBlock(AdderBlock("rightSum"))
        self.addBlock(NegatorBlock("neg"))
        self.addBlock(ProductBlock("leftMult"))
        self.addBlock(ProductBlock("rightMult"))
        self.addBlock(ConstantBlock("WheelRadius", value=(1.0/wheel_radius)))
        self.addBlock(ClampBlock("leftClamp", min=(-p*max_wheel_speed), max=(p*max_wheel_speed)))
        self.addBlock(ClampBlock("rightClamp", min=(-p*max_wheel_speed), max=(p*max_wheel_speed)))

        # Create the Connections
        self.addConnection("steering", "steeringMult", input_port_name='IN1')
        self.addConnection("WheelAxis", "steeringMult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("steeringMult", "halfSteeringMult", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("half", "halfSteeringMult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("velocity", "leftSum", input_port_name='IN1')
        self.addConnection("velocity", "rightSum", input_port_name='IN2')
        self.addConnection("halfSteeringMult", "rightSum", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("halfSteeringMult", "neg", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("neg", "leftSum", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("rightSum", "rightMult", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("leftSum", "leftMult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("WheelRadius", "leftMult", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("WheelRadius", "rightMult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("leftMult", "leftClamp", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("rightMult", "rightClamp", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("rightClamp", "phiRdot", output_port_name='OUT1')
        self.addConnection("leftClamp", "phiLdot", output_port_name='OUT1')


class AGV(CBD):
    def __init__(self, block_name, wheel_radius=(0.03), wheel_axis=(0.18), v=(0.01), Kp=(0.0), Ki=(0.0), Kd=(0.0), T=(50)):
        super().__init__(block_name, input_ports=[], output_ports=[])

        # Create the Blocks
        self.addBlock(ControllerPID("Controller", T=(T), v=(v), Kp=(Kp), Kd=(Kd), Ki=(Ki)))
        self.addBlock(DifferentialDrive("StaticPlant", wheel_axis=(wheel_axis), wheel_radius=(wheel_radius)))
        self.addBlock(ReflectionSensorBlock("colorSensor", port_name=('1')))
        self.addBlock(MotorActuatorBlock("LeftMotor", port_name=('B')))
        self.addBlock(MotorActuatorBlock("RightMotor", port_name=('C')))
        self.addBlock(TCPClientSender("TCP", '192.168.1.10', 8080))

        # Create the Connections
        self.addConnection("Controller", "StaticPlant", output_port_name='heading', input_port_name='steering')
        self.addConnection("Controller", "StaticPlant", output_port_name='velocity', input_port_name='velocity')
        self.addConnection("colorSensor", "Controller", output_port_name='OUT1', input_port_name='color')
        self.addConnection("colorSensor", "TCP", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("StaticPlant", "RightMotor", output_port_name='phiRdot', input_port_name='IN1')
        self.addConnection("StaticPlant", "LeftMotor", output_port_name='phiLdot', input_port_name='IN1')


from CBD.lib.io import WriteCSV, ReadCSV, Interpolation
class AGVVirtual(CBD):
    def __init__(self, block_name, wheel_radius=(0.015), wheel_axis=(0.18), path_file=("paths/falcon.csv"), initial=(0, 0, 0),
                    v=(0.01), T=(50), Kp=(0.0), Kd=(0.0), Ki=(0.0)):
        super().__init__(block_name, input_ports=[], output_ports=[])

        # Create the Blocks
        self.addBlock(ControllerPID("Controller", T=(T), v=(v), Kp=(Kp), Kd=(Kd), Ki=(Ki)))
        self.addBlock(DifferentialDrive("StaticPlant", wheel_axis=(wheel_axis), wheel_radius=(wheel_radius)))
        self.addBlock(PathBlock("environment", path_file=(path_file)))
        self.addBlock(PositionCollectorBlock("plot"))
        self.addBlock(Odometry("odo", initial=(initial)))
        self.addBlock(SignalCollectorBlock("errorPlot"))
        self.addBlock(SignalCollectorBlock("headingPlot"))
        self.addBlock(IntegratorBlock("int"))
        self.addBlock(ConstantBlock("zero", value=(0)))
        self.addBlock(ConstantBlock("delta", value=(DELTA_T)))
        self.addBlock(IntegratorBlock("intH"))
        self.addBlock(ProductBlock("mult"))

        # self.addBlock(WriteCSV("writer", trace_file, ["time", "color", "dur", "tcp_dur", "real"], time_col="arrived_at"))
        # self.addBlock(ServerReceiver("TCP", TCPServerSocket('192.168.1.10', 8080), 1024, initial=[T]))
        # 1 self.addBlock(ReadCSV("TCP", path_file, Interpolation.LINEAR, columns=["color"]))
        # self.addBlock(TCPServerReceiver("TCP", '192.168.1.10', 8080, 1024, initial=[T], convert=int))
        # self.addBlock(Splitter("splitter", 5, lambda x: float(x)))
        # self.addBlock(SignalCollectorBlock("dur"))
        # self.addBlock(SignalCollectorBlock("tcp_dur"))

        # Create the Connections
        self.addConnection("Controller", "StaticPlant", output_port_name='heading', input_port_name='steering')
        self.addConnection("Controller", "StaticPlant", output_port_name='velocity', input_port_name='velocity')
        self.addConnection("environment", "Controller", output_port_name='color', input_port_name='color')
        self.addConnection("StaticPlant", "odo", output_port_name='phiLdot', input_port_name='phiLdot')
        self.addConnection("StaticPlant", "odo", output_port_name='phiRdot', input_port_name='phiRdot')
        self.addConnection("odo", "plot", output_port_name='x', input_port_name='X')
        self.addConnection("odo", "environment", output_port_name='x', input_port_name='x')
        self.addConnection("odo", "plot", output_port_name='y', input_port_name='Y')
        self.addConnection("odo", "environment", output_port_name='y', input_port_name='y')
        self.addConnection("environment", "int", output_port_name='offset', input_port_name='IN1')
        self.addConnection("zero", "int", output_port_name='OUT1', input_port_name='IC')
        self.addConnection("zero", "intH", output_port_name='OUT1', input_port_name='IC')
        self.addConnection("delta", "int", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("delta", "intH", output_port_name='OUT1', input_port_name='delta_t')
        self.addConnection("odo", "intH", output_port_name='heading', input_port_name='IN1')
        self.addConnection("odo", "headingPlot", output_port_name='heading', input_port_name='IN1')
        self.addConnection("intH", "mult", output_port_name='OUT1', input_port_name='IN1')
        self.addConnection("int", "mult", output_port_name='OUT1', input_port_name='IN2')
        self.addConnection("mult", "errorPlot", output_port_name='OUT1', input_port_name='IN1')

        # self.addConnection("odo", "writer", output_port_name="heading", input_port_name="heading")
        # self.addConnection("odo", "writer", output_port_name="x", input_port_name="x")
        # self.addConnection("odo", "writer", output_port_name="y", input_port_name="y")

        # 1 self.addConnection("TCP", "Controller", output_port_name='color', input_port_name='color')

        # self.addConnection("TCP", "splitter", output_port_name='OUT1', input_port_name='IN1')
        # self.addConnection("splitter", "Controller", output_port_name="OUT2", input_port_name="color")
        # self.addConnection("splitter", "writer", output_port_name="OUT1", input_port_name="time")
        # self.addConnection("splitter", "writer", output_port_name="OUT2", input_port_name="color")
        # self.addConnection("splitter", "writer", output_port_name="OUT3", input_port_name="dur")
        # self.addConnection("splitter", "writer", output_port_name="OUT4", input_port_name="tcp_dur")
        # self.addConnection("splitter", "writer", output_port_name="OUT5", input_port_name="real")
        # self.addConnection("splitter", "odo", output_port_name='OUT1', input_port_name='phiLdot')
        # self.addConnection("splitter", "odo", output_port_name='OUT2', input_port_name='phiRdot')




