root/applications/doprava/TrafficAgentLQMaster.h @ 1422

#pragma once
#include "BaseTrafficAgentCt.h"
#include "QuadraticMinimalizator.h"

class TrafficAgentLQMaster :public BaseTrafficAgentCt {
protected:
        vec x;
        QuadraticMinimalizator * minimizer;
public:
        void adapt(const vec &glob_dt) {
                BaseTrafficAgentCt::adapt(glob_dt);
                
                // FUNKCE VOLANE V main_loop V KAZDEM CYKLU
                
                BaseTrafficAgentCt::adapt(glob_dt);

                Q = 100 * diag( ones( queues.length()) ) + diag(computedQueues);
                R = "0.0000001";
                A = diag( ones( queues.length()) );
                B = zeros( queues.length(), 1); 

                // jeden clen matice B
                for ( int i=0; i<queues.length(); i ++ ) {
                        B(i,0) = - s_flow(i) * lanehs(i)->green_time_ratio;
                }
                
                x = getComputedQueues();                
                I0 = (mat)inputs;

        }

        void receive(const Setting& msg){
                string what;
                UI::get(what, msg, "what", UI::compulsory);     

                cout << endl << endl;           

                if ( what.substr(0,9) == "QueueData" ) {
                        vec val;
                        UI::get(val, msg, "value");
                        QueueData qd;
                        qd.fromMessageCode(what);
                        qd.fromVec(val);                        
                        int ind = findQueueDataById( qd.id );
                        if ( ind >= 0 ) {
                                queueData(ind) = qd;
                        } else {
                                ind = queueData.length();
                                queueData.set_length( ind+1, true );
                                queueData(ind) = qd;
                        }                               
                }

                
        }

        void setDummy() {
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        for ( int j = 0; j < queueData(i).outputs.length(); j ++ ) {
                                for ( int k = 0; k < queueData.length(); k ++ ) {
                                        if ( queueData(k).idInput == queueData(i).outputs(j) ) {
                                                queueData(k).dummy = false;
                                        }
                                }
                        }
                }
        }

        void setx() {
                x = ones( queueData.length() + 1 );
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        x(i) = queueData(i).queue;
                }
        }

        // az po setDummy !!!
        void setI0(){
                I0 = zeros(queueData.length(),1);
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        if ( queueData(i).dummy )
                                I0(i,0) = queueData(i).input;
                }
        }

        // az po setI0 !!!
        void setA() {
                A = concat_vertical(
                                concat_horizontal( diag( ones( queueData.length() ) ), I0       ),
                                concat_horizontal( zeros( 1, queueData.length() ) , ones(1,1) )
                        );
        }


        void setB() {
                B = zeros(x.length(), 1);
                //cout << "B' 1 [";
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        B(i,0) = B(i,0) - queueData(i).ss() * queueData(i).green;
                        //cout << B(i,0) << " ";
                }
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        //B(i,0) = B(i,0) - queueData(i).ss() * queueData(i).green;                     
                        for ( int j = 0; j < queueData(i).outputs.length(); j ++ ) {
                                for ( int k = 0; k < queueData.length(); k ++ ) {
                                        if ( queueData(k).idInput == queueData(i).outputs(j) ) {
                                                B(k,0) = B(k,0) + queueData(i).alphas(j) * queueData(i).ss() * queueData(i).green;
                                        }
                                }
                        }
                        
                }
                //cout << endl << "B' 2 " << endl << B.transpose() << endl;
        }

        void setQ() {
                Q = 1000 * diag( ones(x.length()) );
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        Q(i,i) += queueData(i).queue * 100;
                        Q(i,i) += queueData(i).input * 100;
                }
        }

        double computeTc() {
                B = B * T;
                //vec computedQueues = getComputedQueues();
                minimizer = new QuadraticMinimalizator(A, B, Q, R);             
                mat L_mat = minimizer->L( 200);
                vec u_vec = L_mat * x;
                double u = u_vec(0);
                double Tc_computed = L / (1 - u);
                delete minimizer;
                /*
                if ( u >= 1 || Tc_computed > Tc_max ) {
                        cout << endl << "Tc error : " << Tc_computed << " setting to max" << endl;
                        Tc_computed = Tc_max;
                }
                if ( Tc_computed < Tc_min ) {
                        cout << endl << "Tc error : " << Tc_computed << " setting to min" << endl;
                        Tc_computed = Tc_min;   
                }
                */
                return Tc_computed;
        }


        

        void act (vec &glob_ut){                
                BaseTrafficAgentCt::act(glob_ut);
                cout << endl;
                cout << endl;
                cout << "queues ";
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        cout << queueData(i).queue << " ";
                }       
                cout << endl;
                cout << endl;
                cout << "inputs ";
                for ( int i = 0; i < queueData.length(); i ++ ) {
                        cout << queueData(i).input << " ";
                }       
                cout << endl;
                

                setDummy();
                setx();
                setI0();
                setA();
                setB();
                setQ();
                
                R = "0.001";
        
                cout << endl << endl;

                Tc_computed = computeTc();
                cout << "L  = " << L << endl;
                cout << "TC = " << Tc_computed << endl;

                //cout << "A" << endl << A << endl << endl;
                //cout << "I0'" << endl << I0.transpose() << endl << endl;
                

                cout << endl << endl;

                
                
                
                cout << endl << name << " ACT" << endl;

                looper ++;
        }

};
UIREGISTER(TrafficAgentLQMaster);