function [zc,znew,qnew,enew] = IO(x,p,z,q,Gam,FSA)
%[znew,qnew,enew] = IO(x,z,p,q,e,FSA,EV)
%function IO performs input and discrete state updates
%INPUTS: 
%x = current continous state
%p = current static parameter values
%z = current dynamic parameter values
%q = current discrete state of individual physical modules
%Gam = set of allowed controllable events
%FSA = cell array of FSA for physical modules
%
%OUTPUTS:
%znew = vector of dynamic parameter resets
%qnew = vector of FSA state updates
%enew = vector of executed events (for tracking only)


%
%CONTINOUS AND DISCRETE STATES FOR COMPOSITE SYSTEM:
%-parameter vector
%   p = {p1 p2 p3 p4 p5 p6}
%   p{1} = [X1 X2 Vm Vp ns]
%   p{2} = [tauT]
%   p{3} = [tauC]
%   p{4} = [Tp Ps Pt]
%   p{5} = [C nCT]
%   p{6} = [Xs Xm Xr Rs Rr ws Jg]
%-continunous state vector
%   x = [x1 x2 x3 x4 x5 x6]
%   x1 = [V0R V0I V2R V2I V3R V3I Pd Qd IsR IsI IcR IcI]
%   x2 = [xTT yTT]
%   x3 = [xTC yTC]
%   x4 = [xd Pd V3R V3I]
%   x5 = [IcR IcI V2R V2I]
%   x6 = [IsR IsI IrR IrI wr V2R V2I]
%-discrete state vector
%   z = {z1 z2 z3 z4 z5 z6}
%   z{1} = [nT nTq]
%   z{2} = [tresT onT]
%   z{3} = [tresC onC]
%   z{4} = []
%   z{5} = [nC nCq]
%   z{6} = []


qnew = [];
znew = [];
enew = [];
qc = q;
zc = z;
nFSA = length(FSA);
flag = 1;
while flag
    for i = 1:nFSA
        %find intersect between Gam and allowed events in FSA{i}
        eal = intersect(Gam,FSA{i}(qc(i)).events);
        if ~isempty(eal)
            ranE = eal(ceil(rand*length(eal)));
            zc = inputBoard(ranE,x,p,zc);
            qc(i) = executeEvents(FSA{i},ranE,q(i));
            qnew = [qnew qc];
            znew = [znew [zc{:}]'];
            enew = [enew ranE];
        end
    end
    if qc == q
        flag = 0;
    else
        q = qc;
    end
end


function z = inputBoard(e,x,p,z0)
%update of dynamic parameters

z = z0;
nT = p{1}(5);
nC = p{1}(8);

switch e{1}
    case 'tton'
        z{2}(2) = 1;
    case 'ttoff'
        z{2}(1) = x(13);
        z{2}(2) = 0;
    case 'ttstop'
        z{2}(1) = x(13);
        z{2}(2) = 0;
    case 'tcon'
        z{3}(2) = 1;
    case 'tcoff'
        z{3}(1) = x(15);
        z{3}(2) = 0;
    case 'tcstop'
        z{3}(1) = x(15);
        z{3}(2) = 0;
    case 'tm'
        z{1}(2) = z{1}(1) - nT;
    case 'tp'
        z{1}(2) = z{1}(1) + nT;
    case 'cm'
        z{5}(2) = z{5}(1) - nC;
    case 'cmm'
        z{5}(2) = z{5}(1) - 2*nC;
    case 'cp'
        z{5}(2) = z{5}(1) + nC;
    case 'cpp'
        z{5}(2) = z{5}(1) + 2*nC;
    case 'tf'
        z{1}(1) = z{1}(2);
    case 'cf'
        z{5}(1) = z{5}(2);
end

