root/applications/doprava/traffic_agent_LQ.h

#include "BaseTrafficAgentCt.h"
#include "QuadraticMinimalizator.h"

class TrafficAgentLQ : public BaseTrafficAgentCt {
protected:
        QuadraticMinimalizator * minimizer;
        
public:
        // FUNKCE VOLANE V main_loop NA ZACATKU
        
        void from_setting(const Setting& set) {
                BaseTrafficAgentCt::from_setting(set);          
        }

        void validate (){
                BaseTrafficAgentCt::validate();                                 
                
                cout << "HELLO! " << name << " IS HERE!" << endl;
        }

        // FUNKCE VOLANE V main_loop V KAZDEM CYKLU
        void adapt (const vec &glob_dt) {       
                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;
                }

                
        }

        
        void broadcast( Setting &set ) {
                string dummyDet = "DUMMY_DET";
                if ( nOfBroadcast == 0 ) {                      
                        for ( int i = 0; i < lanehs.length(); i++ ) {           
                                for ( int j = 0; j < lanehs(i)->rv_outputs.length(); j ++ ) {
                                        if ( dummyDet.compare(lanehs(i)->rv_outputs.name(j)) != 0 ) {                                           
                                                stringstream coefStr;
                                                coefStr << "coef" << lanehs(i)->rv_outputs.name(j);
                                                double val = s_flow(i) * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j);
                                                sendToAll<double>(set, coefStr.str(), val);
                                        }
                                }                               
                        }
                }


                if ( nOfBroadcast == 1 ) {
                        broadcastTc(set);
                }
                
                nOfBroadcast ++;
        }

        double votingWeight() {
                //vec computedQueues = getComputedQueues();
                double qsum = 0;
                for ( int i = 0; i < computedQueues.length(); i++ ) {
                        qsum += computedQueues(i);
                }
                if ( qsum > 1 )
                        return qsum;
                else
                        return 1;
        }

        void broadcastTc( Setting & set) {
                // posli idealni tc
                Tc_computed = computeTc();
                stringstream tcstr;
                tcstr << "timecycle" << name;
                
                // posli vahu hlasu (suma front)
                //vec computedQueues = getComputedQueues();     
                
                vec tc_vec = "0.0 0.0";
                tc_vec(0) = Tc_computed;
                tc_vec(1) = votingWeight();
                // posilam vektor [tc, w]
                sendToAll<vec>(set, tcstr.str(), tc_vec);
        }

        void receive(const Setting& msg){
                string what;
                UI::get(what, msg, "what", UI::compulsory);             
                if ( what.substr(0,4) == "coef" ) {
                        string inputName = what.substr(4,what.length()-4);
                        int i = find_index(rv_inputs, inputName);
                        double val;
                        UI::get(val, msg, "value");
                        B(i,0) = B(i,0) + val;
                }

                if ( what.substr(0,9) == "timecycle" ) {
                        vec val;
                        UI::get(val, msg, "value");
                        cout << "receiving Tc " << val << endl;
                        Tc_sum += val(0) * val(1);
                        Tc_w_sum += val(1);                     
                }

        }

        void act (vec &glob_ut){                
                //vec computedQueues = getComputedQueues();
                cout << endl << name << " ACT" << endl;
                cout << "Tc computed = " << Tc_computed << endl;
                cout << "queues = " << round( computedQueues )<< endl;                  
                double w = votingWeight();
                Tc_sum += Tc_computed * w;
                Tc_w_sum += w;
                int Tc = (int)round( Tc_sum / Tc_w_sum );               
                setCycleTime(Tc, glob_ut);
                looper ++;
        }
// VYPOCETNI FUNKCE

        // saturovany tok pruhu i
        double s_flow(int i) {
                double min_flow = 0.3;
                double max_flow = 0.5;
                double input = 0;
                if ( 2*i < inputs.length() )
                        input = inputs(2*i);
                
                vec q = getComputedQueues();
                double flow = q(i);
                if ( input > 0 )
                        flow += input;
                flow = flow/T;
                if ( flow > max_flow )
                        return max_flow;
                if ( flow < min_flow )
                        return min_flow;
                else
                        return flow;
        }

        double computeTc() {
                B = B * T;
                //vec computedQueues = getComputedQueues();
                minimizer = new QuadraticMinimalizator(A, B, Q, R);             
                mat L_mat = minimizer->L( 100);
                vec u_vec = L_mat * computedQueues;
                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;
        }

        



        


// KONEC
        ~TrafficAgentLQ() {
                
        }

};

UIREGISTER(TrafficAgentLQ);