function obj = formationControl1stModelBuilder(edgeL,model,kp1,kp2,ki1,ki2,dScale,nNodeVref)
    addpath("./networks-toolbox");
    obj = struct();
    robStateLen = size(model.A,1)
    [obj.R, obj.K, obj.d] = rigidityMatrixFnc(edgeL,robStateLen);
    obj.m = length(obj.d);
    obj.n = numNodes(edgeL);
    obj.nodeStateLength = robStateLen*obj.n;
    obj.stateLength = 2 * obj.nodeStateLength + obj.m;
    _nst = kron(eye(obj.n), model.Nst);
    _kst = kron([zeros(obj.n) eye(obj.n)], model.Kst);
    %obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) eye(obj.n)],model.A) + kron([zeros(obj.n) eye(obj.n);zeros(obj.n) zeros(obj.n)],eye(obj.n));
    % pilih robot mana yang ingin dijalankan dengan mengganti 0 menjadi 1
    %                                                       r  1 2 3                                    r   1 2 3
    %                                                          v v v                                        v v v
    obj.A = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) diag([1 1 1])],model.A) + kron([zeros(obj.n) diag([1 1 1]);zeros(obj.n) zeros(obj.n)],eye(obj.n));
    obj.B = kron([zeros(obj.n); eye(obj.n)],model.B);
    obj.Ksgn = kron([zeros(obj.n) zeros(obj.n);zeros(obj.n) eye(obj.n)],model.K);
    obj.controller = @(set_point,st) arrayfun(model.limit_motor,_nst * set_point + _kst * st(1:(2 * obj.nodeStateLength)));
    obj.formation_controller = @(st) -obj.R(st(1:obj.nodeStateLength),obj.K)'*kp1*(obj.R(st(1:obj.nodeStateLength),obj.K)*st(1:obj.nodeStateLength)-(obj.d*dScale)) -obj.R(st(1:obj.nodeStateLength),obj.K)'*ki1*st(obj.stateLength-obj.m+1:obj.stateLength,1);
    obj.formation_control_dot = @(st) [obj.R(st(1:obj.nodeStateLength),obj.K) zeros(obj.m, obj.nodeStateLength + obj.m)];
    obj.stateA = @(st) [obj.A zeros(obj.stateLength-obj.m, obj.m); obj.formation_control_dot(st)];
    obj.stateB = [obj.B zeros(obj.stateLength-obj.m, obj.m); zeros(obj.m, size(model.B,2)*obj.n) eye(obj.m)];
    obj.stateKsgn = @(st,h) (h*[obj.Ksgn zeros(obj.stateLength-obj.m, obj.m); zeros(obj.m,obj.stateLength)])*arrayfun(model.sgn,st);
    obj.stateInput = @(st) [obj.controller(obj.formation_controller(st), st); obj.d*dScale];

    obj.ss = @(st,vRef) obj.stateA(st)*st +  obj.stateKsgn(st,1) + obj.stateB *obj.stateInput(st);

    obj.dss = @(st,h,vRef) (eye(size(obj.stateA(st))) + (h*obj.stateA(st)))*st  +  obj.stateKsgn(st,h) + (h*obj.stateB)*obj.stateInput(st);
endfunction