1 | |
---|
2 | #include "QuadraticMinimalizator.h" |
---|
3 | |
---|
4 | class TrafficAgentLQ : public BaseTrafficAgent { |
---|
5 | protected: |
---|
6 | mat A, B, Q, R; |
---|
7 | static const int T = 90; |
---|
8 | int L; |
---|
9 | int nOfBroadcast; |
---|
10 | QuadraticMinimalizator * minimizer; |
---|
11 | //map<string, int> queueIndex; |
---|
12 | public: |
---|
13 | // FUNKCE VOLANE V main_loop NA ZACATKU |
---|
14 | |
---|
15 | void validate (){ |
---|
16 | L = 90; |
---|
17 | BaseTrafficAgent::validate(); |
---|
18 | |
---|
19 | rv_action = RV("Tc",1); |
---|
20 | rv_action.add( RV( stage_names, ones_i(stage_names.length()) ) ); |
---|
21 | |
---|
22 | for ( int i=0; i<lanehs.length(); i ++ ) { |
---|
23 | unsigned int index = find_index( green_names, name + "_" + lanehs(i)->getSG() ); |
---|
24 | lanehs(i)->green_time_ratio = green_times( index ); |
---|
25 | } |
---|
26 | |
---|
27 | Q = diag( ones( queues.length()) ); |
---|
28 | R = "1"; |
---|
29 | A = diag( ones( queues.length()) ); |
---|
30 | B = ones( queues.length(), 1); |
---|
31 | |
---|
32 | minimizer = new QuadraticMinimalizator(A,B,Q,R); |
---|
33 | |
---|
34 | cout << "HELLO! " << name << " IS HERE!" << endl; |
---|
35 | /* |
---|
36 | cout << "inputs " << inputs.length() << endl; |
---|
37 | cout << "queues " << queues.length() << endl; |
---|
38 | cout << "rv_inputs" << rv_inputs.length() << endl << rv_inputs << endl; |
---|
39 | cout << "rv_queues" << rv_queues.length() << endl << rv_queues << endl; |
---|
40 | cout << "lanehs " << lanehs.length() << endl << endl; |
---|
41 | cout << "A B Q R " << endl << A << endl << B << endl << Q << endl << R << endl << endl; |
---|
42 | */ |
---|
43 | } |
---|
44 | |
---|
45 | // FUNKCE VOLANE V main_loop V KAZDEM CYKLU |
---|
46 | void adapt (const vec &glob_dt) { |
---|
47 | for ( int i=0; i<lanehs.length(); i ++ ) { |
---|
48 | //lanehs(i)->inputs( 0 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) ); |
---|
49 | //lanehs(i)->inputs( 1 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) + 1 ); |
---|
50 | lanehs(i)->queue = queues( find_index( rv_queues, lanehs(i)->getQueueName() ) ); |
---|
51 | lanehs(i)->countAvgs(); |
---|
52 | } |
---|
53 | |
---|
54 | // jeden clen matice B |
---|
55 | for ( int i=0; i<queues.length(); i ++ ) { |
---|
56 | B(i,0) = 0.5 * lanehs(i)->green_time_ratio; |
---|
57 | } |
---|
58 | |
---|
59 | |
---|
60 | |
---|
61 | BaseTrafficAgent::adapt(glob_dt); |
---|
62 | } |
---|
63 | |
---|
64 | |
---|
65 | void broadcast( Setting &set ) { |
---|
66 | string dummyDet = "DUMMY_DET"; |
---|
67 | if ( nOfBroadcast == 0 ) { |
---|
68 | for ( int i = 0; i < lanehs.length(); i++ ) { |
---|
69 | for ( int j = 0; j < lanehs(i)->rv_outputs.length(); j ++ ) { |
---|
70 | if ( dummyDet.compare(lanehs(i)->rv_outputs.name(j)) != 0 ) { |
---|
71 | stringstream coefStr; |
---|
72 | coefStr << "coef" << lanehs(i)->rv_outputs.name(j); |
---|
73 | double val = 0.5 * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j); |
---|
74 | sendToAll<double>(set, coefStr.str(), val); |
---|
75 | } |
---|
76 | } |
---|
77 | } |
---|
78 | } |
---|
79 | nOfBroadcast ++; |
---|
80 | } |
---|
81 | |
---|
82 | void receive(const Setting& msg){ |
---|
83 | string what; |
---|
84 | UI::get(what, msg, "what", UI::compulsory); |
---|
85 | if ( what.substr(0,4) == "coef" ) { |
---|
86 | string inputName = what.substr(4,what.length()-4); |
---|
87 | int i = find_index(rv_inputs, inputName); |
---|
88 | double val; |
---|
89 | UI::get(val, msg, "value"); |
---|
90 | B(i,0) = B(i,0) - val; |
---|
91 | //cout << name << " received coefs " << inputName << " " << what << endl << val <<endl; |
---|
92 | } |
---|
93 | } |
---|
94 | |
---|
95 | void act (vec &glob_ut){ |
---|
96 | //cout <<"A act"<<endl<<A<<endl; |
---|
97 | B = B * T * L; |
---|
98 | minimizer = new QuadraticMinimalizator(A, B, Q, R); |
---|
99 | vec computedQueues = getComputedQueues(); |
---|
100 | vec res = minimizer->minimize(computedQueues, 100); |
---|
101 | //cout << "minimalization res " << name << endl << res << endl << endl; |
---|
102 | delete minimizer; |
---|
103 | int Tc = 80; |
---|
104 | vec action; |
---|
105 | action.set_size(rv_action._dsize()); |
---|
106 | action(find_index(rv_action, "Tc")) = Tc; |
---|
107 | int st; |
---|
108 | int stage_time_sum = 0; // soucet delky fazi |
---|
109 | action(find_index(rv_action, "Tc")) = Tc; |
---|
110 | for ( int i =0; i < stage_names.length(); i ++) { |
---|
111 | if ( (i+1) < stage_names.length() ) { |
---|
112 | st = round(((double)(stage_times(i)*Tc))/80.0); |
---|
113 | stage_time_sum += st; |
---|
114 | action(find_index(rv_action, stage_names(i))) = st; |
---|
115 | } |
---|
116 | else { // dopocitani posledni faze - oprava zaokrouhlovaci chyby |
---|
117 | action(find_index(rv_action, stage_names(i))) = Tc - stage_time_sum; |
---|
118 | } |
---|
119 | } |
---|
120 | action2ds.filldown(action,glob_ut); |
---|
121 | } |
---|
122 | |
---|
123 | void setCycleTime( int Tc, vec &glob_ut ) { |
---|
124 | |
---|
125 | } |
---|
126 | |
---|
127 | |
---|
128 | |
---|
129 | // POMOCNE FCE |
---|
130 | vec getComputedQueues() { |
---|
131 | vec q = zeros( queues.length() ); |
---|
132 | for ( int i = 0; i < lanehs.length(); i++ ) { |
---|
133 | q(i) = lanehs(i)->queue_avg; |
---|
134 | } |
---|
135 | return q; |
---|
136 | } |
---|
137 | |
---|
138 | template <class T> void sendToAll( Setting &set, string msgCode, T value) { |
---|
139 | for ( int i = 0; i < neighbours.length(); i ++ ) { |
---|
140 | Setting & msg = set.add(Setting::TypeGroup); |
---|
141 | UI::save( neighbours(i), msg, "to" ); |
---|
142 | UI::save (name,msg,"from"); |
---|
143 | UI::save( msgCode, msg, "what" ); |
---|
144 | UI::save( value, msg, "value" ); |
---|
145 | } |
---|
146 | } |
---|
147 | |
---|
148 | void printVector ( RV rv_vector, vec vector, string description ) { |
---|
149 | cout << endl << description << " " << name << endl; |
---|
150 | int k = 0; |
---|
151 | for ( int i = 0; i < rv_vector.length(); i ++ ) { |
---|
152 | cout << rv_vector.name(i) << " : "; |
---|
153 | for ( int j = 0; j < rv_vector.size(i); j ++ ) { |
---|
154 | cout << vector(k) << " "; |
---|
155 | k ++; |
---|
156 | } |
---|
157 | cout << endl; |
---|
158 | } |
---|
159 | cout << endl; |
---|
160 | } |
---|
161 | |
---|
162 | unsigned int find_index ( RV rv_vector, string index_string ) { |
---|
163 | for ( unsigned int i = 0; i < rv_vector.length(); i ++) { |
---|
164 | if ( rv_vector.name(i) == index_string ) |
---|
165 | return i; |
---|
166 | } |
---|
167 | return -1; |
---|
168 | } |
---|
169 | |
---|
170 | unsigned int find_index ( Array <string> arr, string index_string ) { |
---|
171 | for ( unsigned int i = 0; i < arr.length(); i ++) { |
---|
172 | if ( arr(i) == index_string ) |
---|
173 | return i; |
---|
174 | } |
---|
175 | return -1; |
---|
176 | } |
---|
177 | |
---|
178 | }; |
---|
179 | |
---|
180 | UIREGISTER(TrafficAgentLQ); |
---|