function Y_l = load_regressor_UR10E(in1,in2,in3)
%LOAD_REGRESSOR_UR10E
%    Y_L = LOAD_REGRESSOR_UR10E(IN1,IN2,IN3)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    20-Oct-2021 07:53:37

q2 = in1(2,:);
q3 = in1(3,:);
q4 = in1(4,:);
q5 = in1(5,:);
q6 = in1(6,:);
q2d1 = in3(1,:);
q2d2 = in3(2,:);
q2d3 = in3(3,:);
q2d4 = in3(4,:);
q2d5 = in3(5,:);
q2d6 = in3(6,:);
qd1 = in2(1,:);
qd2 = in2(2,:);
qd3 = in2(3,:);
qd4 = in2(4,:);
qd5 = in2(5,:);
qd6 = in2(6,:);
t2 = sin(1.57079632679);
t3 = cos(q2);
t4 = sin(q3);
t5 = cos(q3);
t6 = sin(q2);
t7 = sin(q5);
t8 = cos(q4);
t9 = sin(q4);
t10 = t2.^2;
t11 = qd1.*t2.*t3.*t5;
t24 = qd1.*t2.*t4.*t6;
t12 = t11-t24;
t13 = qd1.*t2.*t3.*t4;
t14 = qd1.*t2.*t5.*t6;
t15 = t13+t14;
t16 = t2.*t3.*t4;
t17 = t2.*t5.*t6;
t18 = t16+t17;
t19 = t2.*t3.*t5;
t21 = t2.*t4.*t6;
t20 = t19-t21;
t22 = t2.*t9.*t18;
t23 = t2.*t8.*t15;
t25 = t2.*t9.*t12;
t26 = t23+t25;
t27 = cos(q5);
t28 = qd2+qd3+qd4;
t29 = t27.*t28;
t41 = t7.*t26;
t30 = qd6+t29-t41;
t40 = t2.*t8.*t20;
t31 = t22-t40;
t32 = t7.*t10.*t30.*t31;
t33 = t7.^2;
t34 = t2.*t8.*t12;
t42 = t2.*t9.*t15;
t35 = t34-t42;
t36 = t2.*t8.*t18;
t37 = t2.*t9.*t20;
t38 = t36+t37;
t39 = t10.*t33.*t35.*t38;
t43 = sin(q6);
t44 = cos(q6);
t45 = t26.*t27;
t46 = t7.*t28;
t47 = t45+t46;
t48 = t38.*t43;
t49 = t26.*t43;
t62 = t27.*t35.*t44;
t50 = t49-t62;
t51 = t7.*t10.*t38.*t50;
t52 = qd5-t34+t42;
t53 = t44.*t47;
t63 = t43.*t52;
t54 = t53-t63;
t55 = t7.*t10.*t31.*t54;
t56 = t31.*t43;
t64 = t27.*t38.*t44;
t57 = t56-t64;
t58 = t7.*t10.*t35.*t57;
t59 = t27.*t31.*t44;
t60 = t48+t59;
t65 = t10.*t30.*t60;
t61 = t51+t55+t58-t65;
t66 = t10.*t27.*t57;
t67 = t10.*t33.*t38.*t44;
t68 = t31.*t44;
t69 = t27.*t38.*t43;
t70 = t68+t69;
t71 = t2.*t27.*t70;
t72 = t44.*t52;
t73 = t43.*t47;
t74 = t72+t73;
t75 = t29-t41;
t76 = t38.*t44;
t84 = t27.*t31.*t43;
t77 = t76-t84;
t78 = t2.*t30.*t77;
t79 = t26.*t44;
t80 = t27.*t35.*t43;
t81 = t79+t80;
t82 = t2.*t7.*t74.*(t22-t40);
t85 = t2.*t7.*t38.*t81;
t86 = t2.*t7.*t35.*t70;
t83 = t78+t82-t85-t86;
t87 = t10.*t50.*t57;
t88 = t27.*t44.*(t22-t40);
t89 = t48+t88;
t90 = t56-t64;
t91 = t2.*t7.*t43.*t57;
t93 = t2.*t7.*t44.*t70;
t92 = t91-t93;
t94 = t2.*t57.*t81;
t95 = t2.*t50.*t70;
t96 = t2.*t74.*t89;
t97 = t74.*t77;
t98 = t81.*(t68+t69);
t99 = t97+t98;
t100 = t68+t69;
t101 = t2.*t3.*t5.*(3.9e1./1.0e3);
t109 = t2.*t4.*t6.*(3.9e1./1.0e3);
t102 = t101-t109;
t103 = t2.*t3.*t4.*(3.9e1./1.0e3);
t104 = t2.*t5.*t6.*(3.9e1./1.0e3);
t105 = t103+t104;
t106 = t2.*t8.*t18.*(2.7e1./2.0e2);
t107 = t2.*t8.*t105;
t108 = t2.*t9.*t20.*(2.7e1./2.0e2);
t110 = t2.*t9.*t102;
t111 = t106+t107+t108+t110;
t112 = t2.*t9.*t18.*(2.7e1./2.0e2);
t113 = t2.*t9.*t105;
t120 = t2.*t8.*t20.*(2.7e1./2.0e2);
t121 = t2.*t8.*t102;
t114 = t112+t113-t120-t121;
t115 = t27.*t114;
t116 = t2.*t3.*(6.13e2./1.0e3);
t117 = t2.*t3.*t5.*(5.71e2./1.0e3);
t122 = t2.*t4.*t6.*(5.71e2./1.0e3);
t118 = t116+t117-t122;
t119 = t7.*t38.*(3.0./2.5e1);
t123 = t7.*t114;
t124 = t27.*t118;
t244 = t27.*t38.*(3.0./2.5e1);
t125 = t123+t124-t244;
t126 = qd2.*(6.13e2./1.0e3);
t127 = qd1.*t2.*t6.*(3.9e1./1.0e3);
t128 = t126+t127;
t129 = qd1.*t2.*t3.*(6.13e2./1.0e3);
t130 = qd1.*t2.*t3.*t5.*(5.71e2./1.0e3);
t155 = qd1.*t2.*t4.*t6.*(5.71e2./1.0e3);
t131 = t129+t130-t155;
t132 = t4.*t128;
t154 = qd1.*t2.*t3.*t5.*(3.9e1./1.0e3);
t133 = t132-t154;
t134 = t2.*t8.*t133;
t135 = t2.*t9.*t15.*(2.7e1./2.0e2);
t136 = qd2.*(5.71e2./1.0e3);
t137 = qd3.*(5.71e2./1.0e3);
t138 = t5.*t128;
t139 = qd1.*t2.*t3.*t4.*(3.9e1./1.0e3);
t140 = t136+t137+t138+t139;
t141 = t2.*t9.*t140;
t156 = t2.*t8.*t12.*(2.7e1./2.0e2);
t142 = t134+t135+t141-t156;
t143 = t26.*t27.*(3.0./2.5e1);
t144 = t7.*t28.*(3.0./2.5e1);
t149 = t7.*t118;
t145 = t115+t119-t149;
t146 = t43.*t145;
t147 = t31.*t43.*(1.17e2./1.0e3);
t148 = t43.*t111;
t150 = t44.*(t22-t40).*(1.17e2./1.0e3);
t151 = t27.*t38.*t43.*(1.17e2./1.0e3);
t152 = t44.*t145;
t153 = t148+t150+t151+t152;
t157 = t2.*t8.*t15.*(2.7e1./2.0e2);
t158 = t2.*t9.*t12.*(2.7e1./2.0e2);
t159 = t2.*t8.*t140;
t192 = t2.*t9.*t133;
t160 = t157+t158+t159-t192;
t161 = t27.*t142;
t162 = t7.*t26.*(3.0./2.5e1);
t249 = t27.*t131;
t250 = t7.*t142;
t163 = t143+t144-t249-t250;
t201 = t44.*t111;
t202 = t27.*t38.*t44.*(1.17e2./1.0e3);
t164 = t146+t147-t201-t202;
t194 = t7.*t131;
t195 = t27.*t28.*(3.0./2.5e1);
t165 = t161+t162-t194-t195;
t166 = t27.*t111;
t167 = t2.*t6.*(6.13e2./1.0e3);
t168 = t2.*t3.*t4.*(5.71e2./1.0e3);
t169 = t2.*t5.*t6.*(5.71e2./1.0e3);
t170 = t167+t168+t169;
t171 = t7.*t170;
t215 = t7.*t31.*(3.0./2.5e1);
t172 = t166+t171-t215;
t183 = qd1.*t2.*t4.*t6.*(3.9e1./1.0e3);
t173 = t154-t183;
t174 = qd1.*t2.*t5.*t6.*(3.9e1./1.0e3);
t175 = t139+t174;
t176 = qd1.*t2.*t6.*(6.13e2./1.0e3);
t177 = qd1.*t2.*t3.*t4.*(5.71e2./1.0e3);
t178 = qd1.*t2.*t5.*t6.*(5.71e2./1.0e3);
t179 = t176+t177+t178;
t180 = t7.*t179;
t181 = t7.*t35.*(3.0./2.5e1);
t182 = t2.*t8.*t175;
t184 = t2.*t9.*t173;
t185 = t157+t158+t182+t184;
t186 = t27.*t185;
t187 = t180+t181+t186;
t188 = t2.*t9.*t175;
t284 = t2.*t8.*t173;
t189 = t135-t156+t188-t284;
t190 = t44.*t52.*(1.17e2./1.0e3);
t191 = t43.*t47.*(1.17e2./1.0e3);
t193 = t43.*t160;
t196 = t44.*t165;
t197 = t190+t191+t193+t196;
t198 = t44.*t47.*(1.17e2./1.0e3);
t199 = t44.*t160;
t222 = t43.*t52.*(1.17e2./1.0e3);
t223 = t43.*t165;
t200 = t198+t199-t222-t223;
t203 = t26.*t43.*(1.17e2./1.0e3);
t204 = t138+t139;
t205 = t177+t178;
t206 = t7.*t205;
t207 = t2.*t8.*t204;
t208 = t157+t158-t192+t207;
t209 = t27.*t208;
t210 = t181+t206+t209;
t211 = t26.*t44.*(1.17e2./1.0e3);
t212 = t2.*t9.*t204;
t213 = t134+t135-t156+t212;
t214 = t27.*t35.*t43.*(1.17e2./1.0e3);
t216 = t38.*t44.*(1.17e2./1.0e3);
t217 = t168+t169;
t218 = t7.*t217;
t219 = t166-t215+t218;
t220 = t38.*t43.*(1.17e2./1.0e3);
t221 = t2.*t77.*t197;
t224 = t2.*t81.*t153;
t225 = t2.*t50.*t164;
t226 = t5.*(6.13e2./1.0e3);
t227 = t226+5.71e2./1.0e3;
t228 = t2.*t4.*t9.*(6.13e2./1.0e3);
t253 = t2.*t8.*t227;
t229 = t228-t253;
t230 = t27.*(3.0./2.5e1);
t231 = t2.*t9.*t227;
t232 = t2.*t4.*t8.*(6.13e2./1.0e3);
t233 = t231+t232;
t254 = t27.*t233;
t234 = t230-t254;
t235 = t44.*t114;
t236 = t166-t215;
t237 = t27.*t31.*t44.*(1.17e2./1.0e3);
t238 = t27.*t160;
t239 = t181+t238;
t240 = t7.*t44.*(1.17e2./1.0e3);
t245 = t2.*t9.*t27.*(5.71e2./1.0e3);
t241 = t230-t245;
t242 = t2.*t7.*t44.*t164;
t243 = t7.*t43.*(1.17e2./1.0e3);
t246 = t43.*t241;
t247 = t2.*t8.*t44.*(5.71e2./1.0e3);
t248 = t240+t246+t247;
t251 = t2.*t27.*t164;
t252 = t7.*(3.0./2.5e1);
t255 = t43.*t234;
t327 = t44.*t229;
t256 = t240+t255-t327;
t257 = t2.*t7.*t43.*t125;
t258 = t43.*t219;
t259 = t44.*(t112+t113-t120-t121);
t260 = t27.*t44.*(t22-t40).*(1.17e2./1.0e3);
t261 = t220+t258+t259+t260;
t262 = t43.*t210;
t263 = t44.*t213;
t278 = t27.*t35.*t44.*(1.17e2./1.0e3);
t264 = t203+t262+t263-t278;
t265 = t27.*t43.*(3.0./2.5e1);
t266 = t240+t265;
t267 = t43.*t125;
t343 = t7.*t38.*t44.*(1.17e2./1.0e3);
t268 = t267-t343;
t269 = t44.*t75.*(1.17e2./1.0e3);
t358 = t43.*t163;
t270 = t269-t358;
t271 = t27.*t35.*(3.0./2.5e1);
t272 = t7.*t111;
t273 = t27.*t31.*(3.0./2.5e1);
t274 = t43.*t236;
t275 = t220+t235+t237+t274;
t276 = t44.*t142;
t277 = t43.*t239;
t279 = t2.*t77.*t163;
t280 = t43.*t172;
t281 = t2.*t7.*t31.*t200;
t282 = t2.*t7.*t35.*t164;
t283 = t43.*t187;
t285 = t44.*t189;
t286 = t27.*t205;
t335 = t7.*t208;
t287 = t271+t286-t335;
t336 = t27.*t217;
t288 = t272+t273-t336;
t289 = t44.*t219;
t311 = t43.*t114;
t313 = t27.*t31.*t43.*(1.17e2./1.0e3);
t290 = t216+t289-t311-t313;
t291 = t44.*t210;
t337 = t43.*t213;
t292 = t211+t214+t291-t337;
t331 = t2.*t7.*t9.*(5.71e2./1.0e3);
t293 = t252-t331;
t294 = t2.*t8.*t43.*(5.71e2./1.0e3);
t332 = t44.*t241;
t295 = t243+t294-t332;
t296 = t44.*t125;
t297 = t7.*t38.*t43.*(1.17e2./1.0e3);
t298 = t296+t297;
t299 = t43.*t75.*(1.17e2./1.0e3);
t300 = t44.*t163;
t301 = t299+t300;
t302 = t10.*t27.*t153;
t328 = t7.*t233;
t303 = t252-t328;
t304 = t7.*t10.*t44.*t125;
t305 = t43.*t229;
t306 = t44.*t234;
t307 = t44.*(t115+t119-t149);
t308 = t148+t150+t151+t307;
t330 = t27.*t44.*(3.0./2.5e1);
t309 = t243-t330;
t310 = t272+t273;
t312 = t44.*t236;
t346 = t7.*t160;
t314 = t271-t346;
t315 = t10.*t89.*t163;
t316 = t44.*t239;
t345 = t43.*t142;
t317 = t211+t214+t316-t345;
t318 = t7.*t10.*t31.*t197;
t319 = t27.*t179;
t340 = t7.*t185;
t320 = t271+t319-t340;
t339 = t27.*t170;
t321 = t272+t273-t339;
t322 = t44.*t172;
t323 = t44.*t187;
t341 = t43.*t189;
t324 = t211+t214+t323-t341;
t325 = t146+t147-t201-t202;
t326 = t123+t124-t244;
t329 = -t243+t305+t306;
t333 = t44.*(t161+t162-t194-t195);
t334 = t190+t191+t193+t333;
t338 = t220+t235+t237+t280;
t342 = t216-t311-t313+t322;
t344 = t203+t276+t277-t278;
t347 = t216-t311+t312-t313;
t348 = t27.^2;
t349 = t10.*t27.*t50;
t350 = t10.*t33.*t35.*t44;
t351 = t349+t350;
t352 = t2.*t27.*t81;
t354 = t2.*t33.*t35.*t43;
t353 = t352-t354;
t355 = t44.^2;
t356 = t2.*t7.*t43.*t50;
t357 = t43.^2;
t359 = t44.*(t135-t156+t188-t284);
t360 = t203-t278+t283+t359;
t361 = t2.*t50.*t256;
t362 = t2.*t81.*t329;
t363 = t2.*t5.*t9.*(6.13e2./1.0e3);
t364 = t232+t363;
t385 = t2.*t5.*t8.*(6.13e2./1.0e3);
t365 = t228-t385;
t366 = t2.*t57.*t256;
t367 = t2.*t7.*t44.*t256;
t368 = t2.*t7.*t38.*t256;
t369 = t251+t257+t368-t2.*t70.*t303;
t370 = t2.*t27.*t256;
t371 = t2.*t7.*t43.*t303;
t372 = t44.*t233;
t412 = t27.*t43.*t229;
t373 = t372-t412;
t374 = t2.*t81.*t303;
t375 = t2.*t3.*t4.*(9.81e2./1.0e2);
t376 = t2.*t5.*t6.*(9.81e2./1.0e2);
t377 = t375+t376;
t378 = t2.*t3.*t5.*(9.81e2./1.0e2);
t380 = t2.*t4.*t6.*(9.81e2./1.0e2);
t379 = t378-t380;
t381 = t2.*t9.*t377;
t382 = t27.*t44.*(1.17e2./1.0e3);
t426 = t43.*t303;
t383 = t382-t426;
t384 = t44.*t364;
t405 = t27.*t43.*t365;
t386 = t384-t405;
t387 = t43.*t364;
t388 = t27.*t43.*(1.17e2./1.0e3);
t389 = t44.*t303;
t390 = t388+t389;
t391 = t10.*t50.*t303;
t392 = t43.*t233;
t393 = t27.*t44.*t229;
t394 = t392+t393;
t395 = t7.*t10.*t35.*t329;
t396 = t10.*t27.*t308;
t397 = t10.*t57.*t303;
t398 = t7.*t10.*t38.*t329;
t399 = t7.*t10.*t44.*t303;
t400 = t2.*t8.*t377;
t401 = t2.*t9.*t379;
t402 = t400+t401;
t421 = t2.*t8.*t379;
t403 = t381-t421;
t404 = -t243+t305+t306;
t406 = t27.*t44.*(t228-t385);
t407 = t387+t406;
t408 = t164.*t256;
t409 = t10.*t125.*t303;
t410 = t10.*t308.*t329;
t411 = t408+t409+t410;
t413 = t2.*t6.*(1.81e2./1.0e3);
t414 = t413-6.13e2./1.0e3;
t415 = t4.*t414;
t422 = t2.*t3.*t5.*(1.81e2./1.0e3);
t416 = t415-t422;
t417 = t5.*t414;
t418 = t2.*t3.*t4.*(1.81e2./1.0e3);
t419 = t417+t418-5.71e2./1.0e3;
t420 = t44.*t402;
t438 = t2.*t4.*t6.*(1.81e2./1.0e3);
t423 = t422-t438;
t424 = t2.*t5.*t6.*(1.81e2./1.0e3);
t425 = t418+t424;
t427 = t7.*(8.7e1./5.0e2);
t428 = t2.*t9.*t416;
t443 = t2.*t8.*t419;
t429 = t428-t443;
t580 = t27.*t429;
t430 = t427-t580;
t431 = t2.*t8.*t416;
t432 = t2.*t9.*t419;
t433 = t431+t432+3.0./2.5e1;
t434 = t43.*t402;
t435 = t27.*t44.*t403;
t436 = t434+t435;
t437 = t2.*t8.*t425;
t439 = t2.*t9.*t423;
t440 = t437+t439;
t441 = t2.*t8.*t423;
t444 = t2.*t9.*t425;
t442 = t441-t444;
t445 = t7.*t10.*t30;
t446 = t10.*t27.*t47;
t447 = t445+t446;
t448 = q2d6.*t10.*t27;
t449 = q2d2.*t10.*t348;
t450 = q2d3.*t10.*t348;
t451 = q2d4.*t10.*t348;
t452 = t7.*t10.*t30.*t35;
t607 = qd5.*t447;
t608 = qd2.*t7.*t10.*t27.*t35;
t609 = qd3.*t7.*t10.*t27.*t35;
t610 = qd4.*t7.*t10.*t27.*t35;
t611 = q2d1.*t7.*t10.*t27.*t38;
t453 = t448+t449+t450+t451+t452-t607-t608-t609-t610-t611;
t454 = qd2.*t351;
t455 = qd3.*t351;
t456 = qd4.*t351;
t457 = t7.*t10.*t54;
t458 = t7.*t10.*t44.*t47;
t612 = t10.*t27.*t30.*t44;
t613 = t10.*t27.*t44.*(t29-t41);
t459 = qd5.*(t457+t458-t612-t613);
t460 = q2d1.*(t66+t67);
t461 = t10.*t27.*t74;
t462 = t7.*t10.*t30.*t43;
t463 = t461+t462;
t464 = qd6.*t463;
t465 = q2d5.*t10.*t27.*t43;
t614 = t10.*t30.*t50;
t615 = t7.*t10.*t35.*t54;
t616 = q2d6.*t7.*t10.*t44;
t617 = q2d2.*t7.*t10.*t27.*t44.*2.0;
t618 = q2d3.*t7.*t10.*t27.*t44.*2.0;
t619 = q2d4.*t7.*t10.*t27.*t44.*2.0;
t466 = t454+t455+t456+t459+t460+t464+t465-t614-t615-t616-t617-t618-t619;
t467 = t2.*t7.*t74;
t468 = t2.*t7.*t43.*t47;
t620 = t2.*t27.*t43.*t75;
t621 = t2.*t27.*t30.*t43;
t469 = t467+t468-t620-t621;
t623 = t2.*t33.*t38.*t43;
t470 = q2d1.*(t71-t623);
t471 = t2.*t27.*t54;
t472 = t2.*t7.*t30.*t44;
t473 = qd6.*(t471+t472);
t474 = qd2.*t353;
t475 = qd3.*t353;
t476 = qd4.*t353;
t477 = t2.*t7.*t35.*t74;
t478 = q2d5.*t2.*t27.*t44;
t479 = q2d6.*t2.*t7.*t43;
t480 = q2d2.*t2.*t7.*t27.*t43.*2.0;
t481 = q2d3.*t2.*t7.*t27.*t43.*2.0;
t482 = q2d4.*t2.*t7.*t27.*t43.*2.0;
t622 = qd5.*t469;
t624 = t2.*t30.*t81;
t483 = t470+t473+t474+t475+t476+t477+t478+t479+t480+t481+t482-t622-t624;
t484 = t7.*t10.*t44.*t74;
t485 = t7.*t10.*t43.*t54;
t486 = t484+t485;
t487 = t7.*t10.*t75.*t355;
t488 = t10.*t27.*t44.*t54;
t489 = qd5.*(t487+t488);
t490 = t10.*t50.*(t53-t63);
t491 = q2d2.*t10.*t33.*t355;
t492 = q2d3.*t10.*t33.*t355;
t493 = q2d4.*t10.*t33.*t355;
t625 = qd6.*t486;
t626 = q2d5.*t7.*t10.*t43.*t44;
t627 = q2d1.*t7.*t10.*t44.*t57;
t628 = qd2.*t7.*t10.*t44.*t50;
t629 = qd3.*t7.*t10.*t44.*t50;
t630 = qd4.*t7.*t10.*t44.*t50;
t494 = t489+t490+t491+t492+t493-t625-t626-t627-t628-t629-t630;
t495 = q2d1.*t92;
t497 = t2.*t7.*t44.*t81;
t496 = qd2.*(t356-t497);
t498 = t2.*t7.*t44.*t54.*2.0;
t633 = t2.*t7.*t43.*t74.*2.0;
t499 = t498-t633;
t500 = t2.*t27.*t44.*t74;
t501 = t2.*t27.*t43.*t54;
t502 = t2.*t7.*t43.*t44.*t75.*2.0;
t503 = t500+t501+t502;
t504 = t2.*t7.*t355;
t634 = t2.*t7.*t357;
t505 = t504-t634;
t506 = t2.*t81.*(t53-t63);
t507 = t7.*t75.*t357;
t508 = t27.*t43.*t74;
t509 = qd5.*(t507+t508);
t510 = t7.*t44.*t74;
t511 = t7.*t43.*(t53-t63);
t512 = t510+t511;
t513 = qd6.*t512;
t514 = q2d2.*t33.*t357;
t515 = q2d3.*t33.*t357;
t516 = q2d4.*t33.*t357;
t517 = qd2.*t7.*t43.*t81;
t518 = qd3.*t7.*t43.*t81;
t519 = qd4.*t7.*t43.*t81;
t520 = q2d5.*t7.*t43.*t44;
t521 = q2d1.*t7.*t43.*(t68+t69);
t636 = t74.*t81;
t522 = t509+t513+t514+t515+t516+t517+t518+t519+t520+t521-t636;
t523 = t2.*t7.*t43.*(t243-t330);
t524 = t2.*t7.*t44.*t248;
t525 = t2.*t7.*t43.*t295;
t526 = t2.*t7.*t44.*t266;
t527 = t2.*t7.*t44.*t264;
t528 = t2.*t50.*t248;
t529 = t2.*t7.*t44.*t360;
t530 = t2.*t7.*t44.*t344;
t639 = t2.*t7.*t43.*t329;
t531 = t367+t524+t525-t639;
t532 = t2.*t57.*t248;
t533 = t2.*t27.*t44.*t200;
t534 = t2.*t27.*t43.*t334;
t535 = t2.*t7.*t44.*t270;
t536 = t2.*t7.*t43.*t301;
t537 = t2.*t50.*t200;
t538 = t2.*t27.*t266;
t539 = t2.*t27.*t248;
t540 = t2.*t33.*t43.*(3.0./2.5e1);
t541 = t2.*t7.*t43.*t293;
t542 = t2.*t7.*t38.*t248;
t543 = t251+t257+t542-t2.*t70.*t293;
t544 = t2.*t27.*t43.*(1.17e2./1.0e3);
t545 = t2.*t9.*t44.*(5.71e2./1.0e3);
t546 = t2.*t8.*t27.*t43.*(5.71e2./1.0e3);
t547 = t545+t546;
t548 = t2.*t7.*t43.*t314;
t593 = t43.*t293;
t549 = t382-t593;
t550 = t2.*t27.*t270;
t551 = t2.*t27.*t43.*t163;
t552 = t2.*t7.*t43.*t287;
t553 = t2.*t7.*t35.*t248;
t554 = t2.*t7.*t44.*t163;
t555 = t370+t371+t539+t541;
t556 = t2.*t81.*t293;
t557 = t2.*t7.*t43.*t320;
t558 = t2.*t7.*t35.*t200;
t559 = t2.*t9.*t43.*(5.71e2./1.0e3);
t592 = t2.*t8.*t27.*t44.*(5.71e2./1.0e3);
t560 = t559-t592;
t561 = t10.*t27.*t317;
t562 = t44.*t293;
t563 = t388+t562;
t564 = t7.*t10.*t334;
t565 = t10.*t27.*t324;
t566 = t10.*t50.*t293;
t567 = t10.*t33.*t44.*(3.0./2.5e1);
t568 = t7.*t10.*t44.*t293;
t569 = t10.*t57.*t293;
t570 = t10.*t27.*t292;
t571 = t10.*t27.*t200;
t572 = t7.*t10.*t43.*t163;
t573 = t10.*t27.*t329;
t574 = t10.*t27.*t44.*(1.17e2./1.0e3);
t575 = t10.*t50.*(t143+t144-t249-t250);
t576 = t417+t418;
t577 = t44.*t403;
t578 = t27.*t43.*t402;
t579 = t577+t578;
t581 = t43.*t430;
t582 = t44.*t433;
t583 = t581+t582;
t660 = t27.*t43.*t403;
t584 = t420-t660;
t585 = t583.*t584;
t586 = t248.*t256;
t587 = t10.*t293.*t303;
t588 = t586+t587-t10.*t295.*t329;
t589 = t164.*t248;
t590 = t10.*t125.*t293;
t591 = t589+t590-t10.*t295.*t308;
t594 = t428-t2.*t8.*t576;
t595 = t2.*t9.*t576;
t596 = t431+t595;
t597 = t43.*t403;
t674 = t27.*t44.*t402;
t598 = t597-t674;
t599 = t44.*t430;
t675 = t43.*t433;
t600 = t599-t675;
t601 = t27.*t44.*(t381-t421);
t602 = t434+t601;
t603 = t27.*(8.7e1./5.0e2);
t604 = t7.*(t428-t443);
t605 = t603+t604+1.17e2./1.0e3;
t606 = t7.*t10.*t605.*(t381-t421);
t631 = qd3.*(t356-t497);
t632 = qd4.*(t356-t497);
t635 = t495+t496+t506+t631+t632-q2d5.*t505-qd6.*t499-qd5.*t503-t2.*t50.*t74-q2d2.*t2.*t33.*t43.*t44.*2.0-q2d3.*t2.*t33.*t43.*t44.*2.0-q2d4.*t2.*t33.*t43.*t44.*2.0;
t637 = t523+t524+t525+t526;
t638 = t2.*t50.*t266;
t640 = t2.*t57.*t266;
t641 = t2.*t7.*(t381-t421);
t642 = t538+t539+t540+t541;
t643 = t2.*t27.*t344;
t644 = t370+t371+t538+t540;
t661 = t7.*t43.*(3.0./2.5e1);
t645 = t382-t661;
t646 = t2.*t27.*t360;
t647 = t2.*t7.*t81.*(3.0./2.5e1);
t648 = t2.*t7.*t38.*t266;
t649 = t251+t257+t648-t2.*t7.*t70.*(3.0./2.5e1);
t650 = t2.*t602;
t651 = t7.*t44.*(3.0./2.5e1);
t652 = t388+t651;
t653 = t10.*t27.*t44.*t163;
t657 = t10.*t27.*t309;
t654 = t567+t568-t657-t10.*t27.*t295;
t655 = t7.*t10.*t50.*(3.0./2.5e1);
t656 = t10.*t27.*(-t243+t305+t306);
t658 = t7.*t10.*t57.*(3.0./2.5e1);
t659 = t304+t396+t658-t7.*t10.*t38.*t309;
t662 = t243-t330;
t663 = t256.*t266;
t664 = t7.*t10.*t303.*(3.0./2.5e1);
t665 = t663+t664-t10.*t309.*t329;
t666 = t164.*t266;
t667 = t7.*t10.*t125.*(3.0./2.5e1);
t668 = t666+t667-t10.*t308.*t309;
t669 = t248.*t266;
t670 = t10.*t295.*(t243-t330);
t671 = t7.*t10.*t293.*(3.0./2.5e1);
t672 = t669+t670+t671;
t673 = t431+t432;
t676 = t2.*t44.*t57;
t677 = t2.*t43.*t70;
t678 = t2.*t44.*t50;
t679 = t2.*t43.*t81;
t680 = t678+t679;
t681 = t2.*t43.*t256;
t682 = t2.*t44.*t329;
t683 = t681+t682;
t684 = t2.*t43.*t248;
t685 = t684-t2.*t44.*t295;
t686 = t2.*t43.*t266;
t687 = t686-t2.*t44.*t309;
t688 = t2.*t43.*t164;
t689 = t2.*t44.*t70.*(1.17e2./1.0e3);
t690 = t2.*t43.*t57.*(1.17e2./1.0e3);
t691 = t2.*t44.*t81.*(1.17e2./1.0e3);
t692 = t2.*t43.*t50.*(1.17e2./1.0e3);
t693 = t2.*t44.*t125;
t694 = t693-t2.*t7.*t38.*t43.*(1.17e2./1.0e3);
t695 = t544-t2.*t44.*t293;
t696 = t2.*t7.*t35.*t43.*(1.17e2./1.0e3);
t697 = t544-t2.*t44.*t303;
t698 = t544-t2.*t7.*t44.*(3.0./2.5e1);
t699 = t10.*t43.*t303;
t700 = t574+t699;
t701 = t7.*t10.*t43.*(3.0./2.5e1);
t702 = t574+t701;
t703 = t10.*t43.*t125;
t704 = t7.*t10.*t38.*t44.*(1.17e2./1.0e3);
t705 = t10.*t43.*t293;
t706 = t574+t705;
t707 = t43.*t256.*(1.17e2./1.0e3);
t708 = t10.*t44.*t329.*(1.17e2./1.0e3);
t709 = t707+t708;
t710 = t43.*t248.*(1.17e2./1.0e3);
t711 = t710-t10.*t44.*t295.*(1.17e2./1.0e3);
t712 = t43.*t266.*(1.17e2./1.0e3);
t713 = t712-t10.*t44.*t309.*(1.17e2./1.0e3);
t714 = t43.*t164.*(1.17e2./1.0e3);
t715 = t10.*t44.*t308.*(1.17e2./1.0e3);
t716 = t603+t604;
t717 = t53-t63;
Y_l = reshape([qd2.*(t32+t39)+qd3.*(t32+t39)+qd4.*(t32+t39)+qd5.*(t7.*t10.*t38.*t47-t10.*t27.*t30.*t38)-q2d6.*t7.*t10.*t38+q2d1.*t10.*t33.*t38.^2-q2d2.*t7.*t10.*t27.*t38-q2d3.*t7.*t10.*t27.*t38-q2d4.*t7.*t10.*t27.*t38,t453,t453,t453,t10.*t30.*t47,q2d6.*t10-(qd5.*t10.*(t7.*t28.*2.0+t26.*t27.*2.0))./2.0+q2d2.*t10.*t27+q2d3.*t10.*t27+q2d4.*t10.*t27-q2d1.*t7.*t10.*t38-qd2.*t7.*t10.*t35-qd3.*t7.*t10.*t35-qd4.*t7.*t10.*t35,q2d2.*(t66+t67)+q2d3.*(t66+t67)+q2d4.*(t66+t67)+qd6.*(t10.*t30.*t70-t7.*t10.*t38.*t74)-qd2.*t61-qd3.*t61-qd4.*t61+qd5.*(-t10.*t47.*t57+t10.*t27.*t38.*(t53-t63)+t7.*t10.*t30.*t38.*t44+t7.*t10.*t38.*t44.*t75)+q2d6.*t10.*(t56-t64)-q2d5.*t7.*t10.*t38.*t43-q2d1.*t7.*t10.*t38.*t57.*2.0,t466,t466,t466,q2d6.*t10.*t43-t10.*t47.*t54+q2d2.*t10.*t27.*t43+q2d3.*t10.*t27.*t43+q2d4.*t10.*t27.*t43+qd6.*t10.*t30.*t44-qd5.*t10.*t43.*t47+t10.*t30.*t44.*(t29-t41)-q2d1.*t7.*t10.*t38.*t43-qd2.*t7.*t10.*t35.*t43-qd3.*t7.*t10.*t35.*t43-qd4.*t7.*t10.*t35.*t43,q2d5.*t10.*t43+qd2.*t10.*t50+qd3.*t10.*t50+qd4.*t10.*t50+qd6.*t10.*t74-t10.*t30.*t74+q2d1.*t10.*(t56-t64)-q2d2.*t7.*t10.*t44-q2d3.*t7.*t10.*t44-q2d4.*t7.*t10.*t44-qd5.*t10.*t44.*t75,-qd6.*(t2.*t30.*t57+t2.*t7.*t38.*t54)+qd2.*t83+qd3.*t83+qd4.*t83-qd5.*(t2.*t47.*t70+t2.*t27.*t38.*t74+t2.*t7.*t30.*t38.*t43+t2.*t7.*t38.*t43.*t75)+q2d2.*(t71-t2.*t33.*t38.*t43)+q2d3.*(t71-t2.*t33.*t38.*t43)+q2d4.*(t71-t2.*t33.*t38.*t43)+q2d6.*t2.*(t68+t69)-q2d5.*t2.*t7.*t38.*t44-q2d1.*t2.*t7.*t38.*t70.*2.0,t483,t483,t483,q2d6.*t2.*t44+t2.*t47.*t74+q2d2.*t2.*t27.*t44+q2d3.*t2.*t27.*t44+q2d4.*t2.*t27.*t44-qd6.*t2.*t30.*t43-qd5.*t2.*t44.*t47-t2.*t30.*t43.*t75-q2d1.*t2.*t7.*t38.*t44-qd2.*t2.*t7.*t35.*t44-qd3.*t2.*t7.*t35.*t44-qd4.*t2.*t7.*t35.*t44,q2d1.*t2.*(t68+t69)+q2d5.*t2.*t44+qd2.*t2.*t81+qd3.*t2.*t81+qd4.*t2.*t81-t2.*t30.*t54+qd6.*t2.*(t53-t63)+q2d2.*t2.*t7.*t43+q2d3.*t2.*t7.*t43+q2d4.*t2.*t7.*t43+qd5.*t2.*t43.*(t29-t41),-qd5.*(t10.*t44.*t57.*t75+t7.*t10.*t38.*t44.*t54)+qd2.*(t87-t10.*t54.*t60)+qd3.*(t87-t10.*t89.*(t53-t63))+qd4.*(t87-t10.*t89.*(t53-t63))+qd6.*(t10.*t57.*t74-t10.*(t68+t69).*(t53-t63))+q2d1.*t10.*t90.^2+q2d5.*t10.*t43.*(t56-t64)-q2d2.*t7.*t10.*t44.*t57-q2d3.*t7.*t10.*t44.*t57-q2d4.*t7.*t10.*t44.*t57,t494,t494,t494,-qd6.*(t10.*t44.*t54-t10.*t43.*t74)+q2d5.*t10.*t357+qd2.*t10.*t43.*t50+qd3.*t10.*t43.*t50+qd4.*t10.*t43.*t50-t10.*t44.*t54.*t75+q2d1.*t10.*t43.*(t56-t64)-q2d2.*t7.*t10.*t43.*t44-q2d3.*t7.*t10.*t43.*t44-q2d4.*t7.*t10.*t43.*t44-qd5.*t10.*t43.*t44.*t75,t10.*t74.*(t53-t63),q2d5.*(t676+t677)+q2d2.*t92+q2d3.*t92+q2d4.*t92+qd2.*(t94+t95+t96-t2.*t77.*(t53-t63))+qd3.*(t94+t95+t96-t2.*t77.*(t53-t63))+qd4.*(t94+t95+t96-t2.*t77.*(t53-t63))+qd6.*(t2.*t54.*t57.*2.0+t2.*t70.*t74.*2.0)+qd5.*(t2.*t43.*t57.*t75-t2.*t44.*(t68+t69).*(t29-t41)+t2.*t7.*t38.*t43.*t54+t2.*t7.*t38.*t44.*t74)+q2d1.*t2.*(t68+t69).*(t56-t64).*2.0,t495+t496+t506-q2d5.*t505-qd6.*t499-qd5.*t503+qd3.*(t356-t2.*t7.*t44.*t81)+qd4.*(t356-t2.*t7.*t44.*t81)-t2.*t50.*t74-q2d2.*t2.*t33.*t43.*t44.*2.0-q2d3.*t2.*t33.*t43.*t44.*2.0-q2d4.*t2.*t33.*t43.*t44.*2.0,t635,t635,q2d1.*(t676+t677)-q2d2.*t505-q2d3.*t505-q2d4.*t505+qd2.*t680+qd3.*t680+qd4.*t680-qd5.*(t2.*t75.*t355-t2.*t75.*t357)+qd6.*(t2.*t44.*t74.*2.0+t2.*t43.*(t53-t63).*2.0)+q2d5.*t2.*t43.*t44.*2.0+t2.*t44.*t74.*(t29-t41)+t2.*t43.*(t29-t41).*(t53-t63),-t2.*t74.^2+t2.*t717.^2,qd5.*(t43.*(t68+t69).*(t29-t41)-t7.*t38.*t43.*t74)+qd2.*t99+qd3.*t99+qd4.*t99+q2d1.*t100.^2+qd6.*(t54.*t70-t57.*t74)+q2d5.*t44.*(t68+t69)+q2d2.*t7.*t43.*(t68+t69)+q2d3.*t7.*t43.*(t68+t69)+q2d4.*t7.*t43.*(t68+t69),t522,t522,t522,q2d5.*t355+qd6.*(t44.*t54-t43.*t74)+q2d1.*t44.*(t68+t69)+qd2.*t44.*t81+qd3.*t44.*t81+qd4.*t44.*t81-t43.*t74.*t75+q2d2.*t7.*t43.*t44+q2d3.*t7.*t43.*t44+q2d4.*t7.*t43.*t44+qd5.*t43.*t44.*(t29-t41),-t54.*t74,-q2d3.*(t242+t532-t2.*t70.*t295-t2.*t7.*t43.*t153)-q2d4.*(t242+t640-t2.*t70.*t309-t2.*t7.*t43.*t153)-q2d2.*(t242+t366+t2.*t70.*(t305+t306-t7.*t43.*(1.17e2./1.0e3))-t2.*t7.*t43.*t153)+qd4.*(t221+t224+t225+t2.*t74.*(t216+t312-t43.*t114-t27.*t31.*t43.*(1.17e2./1.0e3))-t2.*t89.*t200+t2.*t70.*t317+t2.*t57.*(t203+t276+t277-t27.*t35.*t44.*(1.17e2./1.0e3))-t2.*t275.*(t53-t63))+qd2.*(t221+t224+t225+t2.*t74.*(t216+t322-t43.*t114-t27.*t31.*t43.*(1.17e2./1.0e3))-t2.*t89.*t200+t2.*t70.*t324+t2.*t57.*(t203+t283+t285-t27.*t35.*t44.*(1.17e2./1.0e3))-t2.*t338.*(t53-t63))+q2d5.*(t688+t689+t690+t2.*t44.*t153)+qd3.*(t221+t224+t225-t2.*t89.*t200+t2.*t57.*t264+t2.*t70.*t292+t2.*t74.*t290-t2.*t261.*(t53-t63))+q2d1.*(t2.*t153.*(t68+t69).*2.0+t2.*t57.*t164.*2.0)-qd5.*(-t2.*t54.*t268+t2.*t57.*t270-t2.*t70.*t301+t2.*t74.*t298-t2.*t43.*t75.*t153+t2.*t44.*t75.*t164+t2.*t7.*t38.*t43.*t197+t2.*t7.*t38.*t44.*t200),t537-q2d1.*(t242+t366+t2.*t70.*t329-t2.*t7.*t43.*t308)-qd4.*(t361+t362+t530+t2.*t54.*t373+t2.*t74.*t394-t2.*t7.*t43.*t317)+q2d3.*t531-q2d5.*t683-qd3.*(t361+t362+t527+t2.*t54.*t386+t2.*t74.*(t387+t27.*t44.*t365)-t2.*t7.*t43.*t292)+q2d2.*(t2.*t7.*t44.*t256.*2.0-t2.*t7.*t43.*t329.*2.0)+qd5.*(t533+t534+t535+t536+t2.*t54.*t383+t2.*t74.*t390-t2.*t43.*(t29-t41).*(-t243+t305+t306)+t2.*t44.*t75.*t256)+q2d4.*(t367+t523+t526-t2.*t7.*t43.*t329)-qd2.*(t361+t362+t529-t2.*t7.*t43.*t324)-t2.*t74.*t324+t2.*t7.*t403-t2.*t81.*t334+t2.*t360.*(t53-t63),t537+t641-q2d1.*(t242+t532-t2.*t70.*t295-t2.*t7.*t43.*t308)-qd3.*(t527+t528-t2.*t81.*t295-t2.*t7.*t43.*t292)-qd2.*(t528+t529-t2.*t81.*t295-t2.*t7.*t43.*t324)+q2d2.*t531+q2d4.*t637-q2d5.*t685-qd4.*(t528+t530-t2.*t81.*t295+t2.*t54.*t547+t2.*t74.*t560-t2.*t7.*t43.*t317)+q2d3.*(t2.*t7.*t44.*t248.*2.0+t2.*t7.*t43.*t295.*2.0)+qd5.*(t533+t534+t535+t536+t2.*t54.*t549+t2.*t74.*t563+t2.*t44.*t75.*t248+t2.*t43.*t75.*t295)-t2.*t74.*t292-t2.*t81.*t334+t2.*t264.*(t53-t63),t537+t641-q2d1.*(t242+t640-t2.*t70.*t309-t2.*t7.*t43.*t308)-qd3.*(t527+t638-t2.*t81.*t309-t2.*t7.*t43.*t292)-qd4.*(t530+t638-t2.*t81.*t309-t2.*t7.*t43.*t317)-qd2.*(t529+t638-t2.*t81.*t309-t2.*t7.*t43.*t324)+qd5.*(t533+t534+t2.*t74.*t652+t2.*t645.*(t53-t63)+t2.*t7.*t43.*(t299+t300)+t2.*t7.*t44.*(t269-t43.*(t143+t144-t249-t250))+t2.*t44.*t266.*(t29-t41)+t2.*t43.*(t29-t41).*(t243-t330))+q2d3.*t637-q2d5.*t687+q2d2.*(t367+t523+t526-t639)+q2d4.*(t2.*t7.*t44.*t266.*2.0+t2.*t7.*t43.*t309.*2.0)-t2.*t74.*t317-t2.*t81.*t334+t2.*t344.*(t53-t63),-q2d2.*t683-q2d3.*t685-q2d4.*t687+q2d1.*(t688+t689+t690+t2.*t44.*t308)+qd5.*(t2.*t44.*(t299+t300)-t2.*t43.*t270)+qd3.*(t691+t692+t2.*t43.*t264+t2.*t44.*t292)+qd4.*(t691+t692+t2.*t44.*t317+t2.*t43.*t344)+qd2.*(t691+t692+t2.*t44.*t324+t2.*t43.*t360)+q2d5.*(t2.*t355.*(1.17e2./5.0e2)+t2.*t357.*(1.17e2./5.0e2))-t2.*t54.*t270-t2.*t74.*t301-t2.*t27.*t402-t2.*t44.*t75.*t200-t2.*t43.*t75.*t334,0.0,-q2d1.*(t2.*t70.*t125.*2.0-t2.*t7.*t38.*t164.*2.0)-q2d2.*t369-q2d3.*t543-q2d4.*t649-q2d5.*t694-qd5.*(-t2.*t47.*t164+t2.*t74.*t145+t2.*t70.*t165-t2.*t30.*t268+t2.*t43.*t75.*t125+t2.*t27.*t38.*t200+t2.*t7.*t38.*t270+t2.*t7.*t38.*t43.*t163)+qd3.*(t279+t281+t282-t2.*t81.*(t123+t124-t244)-t2.*t30.*t261+t2.*t70.*t287-t2.*t74.*t288+t2.*t7.*t38.*t264)+qd4.*(t2.*t77.*(t143+t144-t249-t250)+t2.*t314.*(t68+t69)-t2.*t81.*t125-t2.*t30.*t275-t2.*t74.*t310+t2.*t7.*t38.*t344+t2.*t7.*t200.*(t22-t40)+t2.*t7.*t35.*(t146+t147-t201-t202))+qd2.*(t279+t281+t282-t2.*t81.*(t123+t124-t244)-t2.*t30.*t338+t2.*t70.*t320-t2.*t74.*t321+t2.*t7.*t38.*(t203-t278+t283+t285))-qd6.*(t2.*t54.*t125+t2.*t30.*t153+t2.*t57.*t163-t2.*t7.*t38.*t197)-q2d6.*t2.*t164,t558+qd2.*(t374+t557-t2.*t27.*t360-t2.*t7.*t35.*t256)+q2d2.*(t2.*t27.*t256.*2.0+t2.*t7.*t43.*t303.*2.0)-q2d1.*t369+q2d3.*t555+q2d4.*t644-q2d5.*t697+t2.*t436+qd6.*(t554+t2.*t54.*t303+t2.*t30.*t329-t2.*t27.*t334)+qd5.*(t550+t551-t2.*t7.*t200-t2.*t47.*t256+t2.*t74.*t234+t2.*t30.*t383+t2.*t43.*t75.*t303-t2.*t7.*t43.*(t161+t162-t194-t195))+qd4.*(t374+t548-t2.*t27.*t344-t2.*t30.*t373-t2.*t7.*t35.*t256+t2.*t7.*t74.*t229)+qd3.*(t374+t552-t2.*t27.*t264-t2.*t30.*t386-t2.*t7.*t35.*t256+t2.*t7.*t74.*t365)+q2d6.*t2.*t256-t2.*t81.*t163+t2.*t30.*t360-t2.*t74.*t320,t558+t650+q2d3.*(t2.*t27.*t248.*2.0+t2.*t7.*t43.*t293.*2.0)-q2d1.*t543+q2d2.*t555+q2d4.*t642-q2d5.*t695-qd4.*(-t548+t553+t643-t2.*t81.*t293+t2.*t30.*t547+t7.*t8.*t10.*t74.*(5.71e2./1.0e3))+qd3.*(t552-t553+t556-t2.*t27.*t264)-qd2.*(t553-t556-t557+t646)+qd6.*(t554-t2.*t30.*t295+t2.*t54.*t293-t2.*t27.*t334)+qd5.*(t550+t551-t2.*t7.*t200-t2.*t47.*t248+t2.*t74.*t241+t2.*t30.*t549+t2.*t43.*t75.*t293-t2.*t7.*t43.*(t161+t162-t194-t195))+q2d6.*t2.*t248-t2.*t81.*t163+t2.*t30.*t264-t2.*t74.*t287,t558+t650+qd3.*(t552+t647-t2.*t27.*t264-t2.*t7.*t35.*t266)+qd6.*(t554+t2.*t7.*t54.*(3.0./2.5e1)-t2.*t27.*t334-t2.*t30.*(t243-t330))+qd4.*(t548-t643+t647-t2.*t7.*t35.*t266)+qd2.*(t557-t646+t647-t2.*t7.*t35.*t266)+q2d3.*t642+q2d2.*t644-q2d1.*t649-q2d5.*t698+q2d4.*(t2.*t33.*t43.*(6.0./2.5e1)+t2.*t27.*t266.*2.0)+qd5.*(t550+t551+t2.*t27.*t74.*(3.0./2.5e1)-t2.*t7.*t200-t2.*t47.*t266+t2.*t30.*t645+t2.*t7.*t43.*t75.*(3.0./2.5e1)-t2.*t7.*t43.*(t161+t162-t194-t195))+q2d6.*t2.*t266-t2.*t81.*t163+t2.*t30.*t344-t2.*t74.*t314,-q2d1.*t694-q2d3.*t695-q2d2.*t697-q2d4.*t698+qd3.*(t696+t2.*t44.*t287)+qd4.*(t696+t2.*t44.*t314)+qd2.*(t696+t2.*t44.*t320)-qd6.*(t2.*t30.*t44.*(1.17e2./1.0e3)+t2.*t43.*t163)+qd5.*(t2.*t43.*t47.*(1.17e2./1.0e3)-t2.*t44.*t165)+t2.*t74.*(t161+t162-t194-t195)-q2d6.*t2.*t43.*(1.17e2./1.0e3)+t2.*t47.*t200-t2.*t30.*t270-t2.*t43.*t75.*t163+t2.*t7.*t44.*t402,t2.*(t577+t578)-q2d5.*t2.*t43.*(1.17e2./1.0e3)-q2d1.*t2.*t164+q2d3.*t2.*t248+q2d2.*t2.*t256+q2d4.*t2.*t266-qd3.*t2.*t264-qd6.*t2.*t334-qd4.*t2.*t344-qd2.*t2.*t360-t2.*t54.*t163+t2.*t30.*t334+qd5.*t2.*(t269-t43.*(t143+t144-t249-t250)),q2d5.*(t703+t704)-q2d4.*t659-qd3.*(t315+t318-t10.*t50.*t125+t10.*t30.*t290+t10.*t54.*t288+t10.*t57.*t287-t7.*t10.*t35.*t153-t7.*t10.*t38.*t292)-qd4.*(t315+t318-t10.*t50.*t125+t10.*t54.*t310+t10.*t57.*t314+t10.*t30.*t347-t7.*t10.*t35.*t308-t7.*t10.*t38.*t317)-qd2.*(t315+t318-t10.*t50.*t125+t10.*t30.*t342+t10.*t54.*t321+t10.*t57.*t320-t7.*t10.*t35.*t308-t7.*t10.*t38.*t324)+qd5.*(-t10.*(t53-t63).*(t115+t119-t149)+t10.*t47.*t153+t10.*t57.*t165+t10.*t30.*t298-t10.*t44.*(t29-t41).*(t123+t124-t244)+t10.*t27.*t38.*t197+t7.*t10.*t38.*t301-t7.*t10.*t38.*t44.*(t143+t144-t249-t250))-q2d3.*(t302+t304+t569-t7.*t10.*t38.*t295)+qd6.*(-t10.*(t68+t69).*(t143+t144-t249-t250)+t10.*t30.*t164+t10.*t74.*t125+t7.*t10.*t38.*t200)+q2d1.*(t10.*(t56-t64).*(t123+t124-t244).*2.0+t7.*t10.*t38.*t308.*2.0)-q2d2.*(t302+t304+t397+t398)-q2d6.*t10.*t308,t420+t575-qd3.*(t570+t10.*t50.*t303-t10.*t30.*t407-t7.*t10.*t44.*t287+t7.*t10.*t35.*t329-t7.*t10.*(t53-t63).*(t228-t385))+q2d2.*(t10.*t27.*t329.*2.0+t7.*t10.*t44.*t303.*2.0)-q2d5.*t700+q2d3.*(t399+t568+t573-t10.*t27.*t295)+q2d4.*(t399+t567+t656-t10.*t27.*t309)+qd5.*(t564+t653+t10.*t54.*t234-t10.*t27.*t301-t10.*t47.*t329-t10.*t30.*t390-t7.*t10.*t44.*t165+t10.*t44.*t75.*t303)-qd6.*(t571+t572+t10.*t30.*t256+t10.*t74.*t303)-qd4.*(t391+t395+t561-t10.*t30.*t394-t7.*t10.*t54.*t229-t7.*t10.*t44.*t314)-qd2.*(t391+t395+t565-t7.*t10.*t44.*t320)-q2d1.*(t304+t396+t397+t398)+q2d6.*t10.*(-t243+t305+t306)+t10.*t30.*t324-t10.*t54.*t320-t27.*t43.*t403-t7.*t10.*t35.*t334,t420+t575+qd5.*(t564-t10.*t27.*t301+t10.*t47.*t295-t10.*t30.*t563+t10.*t241.*(t53-t63)-t7.*t10.*t44.*t165+t10.*t44.*t293.*(t29-t41)+t10.*t27.*t44.*(t143+t144-t249-t250))-q2d3.*(t10.*t27.*t295.*2.0-t7.*t10.*t44.*t293.*2.0)+q2d4.*t654-q2d5.*t706+q2d2.*(t399+t568+t573-t10.*t27.*t295)-qd3.*(t566+t570-t7.*t10.*t35.*t295-t7.*t10.*t44.*t287)-qd2.*(t565+t566-t7.*t10.*t35.*t295-t7.*t10.*t44.*t320)-qd6.*(t571+t572+t10.*t30.*t248+t10.*t74.*t293)-q2d1.*(t304+t396+t569-t7.*t10.*t38.*t295)-qd4.*(t561+t566-t10.*t30.*t560-t7.*t10.*t35.*t295-t7.*t10.*t44.*t314+t2.*t7.*t8.*t10.*t54.*(5.71e2./1.0e3))-q2d6.*t10.*t295+t10.*t30.*t292-t10.*t54.*t287-t27.*t43.*t403-t7.*t10.*t35.*t334,t420+t575-t660+q2d3.*t654-q2d1.*t659-q2d5.*t702+q2d2.*(t399+t567+t656-t657)-qd3.*(t570+t655-t7.*t10.*t44.*t287-t7.*t10.*t35.*t309)-qd4.*(t561+t655-t7.*t10.*t35.*t309-t7.*t10.*t44.*t314)-qd2.*(t565+t655-t7.*t10.*t35.*t309-t7.*t10.*t44.*t320)-qd6.*(t571+t572+t7.*t10.*t74.*(3.0./2.5e1)+t10.*t30.*t266)+q2d4.*(t10.*t33.*t44.*(6.0./2.5e1)-t10.*t27.*t309.*2.0)+qd5.*(t564+t653-t10.*t27.*(t299+t300)+t10.*t27.*t54.*(3.0./2.5e1)+t10.*t47.*t309-t10.*t30.*t652+t7.*t10.*t44.*t75.*(3.0./2.5e1)-t7.*t10.*t44.*(t161+t162-t194-t195))-q2d6.*t10.*t309+t10.*t30.*t317-t10.*t54.*t314-t7.*t10.*t35.*t334,q2d1.*(t703+t704)-qd3.*(t10.*t43.*t287-t7.*t10.*t35.*t44.*(1.17e2./1.0e3))-qd4.*(t10.*t43.*t314-t7.*t10.*t35.*t44.*(1.17e2./1.0e3))-qd2.*(t10.*t43.*t320-t7.*t10.*t35.*t44.*(1.17e2./1.0e3))-q2d2.*t700-q2d4.*t702-q2d3.*t706+qd6.*(t10.*t30.*t43.*(1.17e2./1.0e3)-t10.*t44.*t163)+qd5.*(t10.*t44.*t47.*(1.17e2./1.0e3)+t10.*t43.*t165)+t10.*(t53-t63).*(t161+t162-t194-t195)+t10.*t30.*(t299+t300)-q2d6.*t10.*t44.*(1.17e2./1.0e3)-t10.*t47.*t334-t7.*t43.*t402-t10.*t44.*t75.*t163,-t597+t674+t10.*t74.*(t143+t144-t249-t250)+q2d2.*t10.*(-t243+t305+t306)-q2d5.*t10.*t44.*(1.17e2./1.0e3)-q2d3.*t10.*t295-q2d1.*t10.*t308-q2d4.*t10.*t309-qd6.*t10.*t200-qd3.*t10.*t292-qd5.*t10.*t301-qd4.*t10.*t317-qd2.*t10.*t324+t10.*t30.*t200,q2d5.*(t714+t715)-qd2.*(t200.*t338-t360.*(t146+t147-t201-t202)+t10.*t125.*t320+t10.*t163.*t321-t10.*t308.*t324-t10.*t334.*t342)+qd5.*(t200.*t268-(t269-t43.*(t143+t144-t249-t250)).*(t146+t147-t201-t202)-t10.*t334.*(t296+t297)+t10.*t125.*t165+t10.*t301.*t308-t10.*(t115+t119-t149).*(t143+t144-t249-t250))-q2d2.*t411-q2d3.*t591-q2d4.*t668+q2d1.*(t10.*t308.^2+t10.*t326.^2+t325.^2)+qd3.*(t164.*t264-t200.*t261-t10.*t287.*(t123+t124-t244)+t10.*t292.*t308+t10.*t290.*t334-t10.*(t272+t273-t336).*(t143+t144-t249-t250))-qd4.*(t200.*t275-t164.*t344+t10.*t125.*t314+t10.*t163.*t310-t10.*t308.*t317-t10.*t334.*t347)+qd6.*(t164.*t197-t200.*t308-t10.*t334.*(t146+t147-t201-t202)+t10.*t200.*t308),t585+t606-qd4.*(t200.*t373+t256.*t344-t10.*t303.*t314+t10.*t317.*t329+t10.*t334.*t394-t7.*t10.*t163.*t229)-qd3.*(t256.*t264+t200.*t386-t10.*t287.*t303+t10.*t292.*t329+t10.*t334.*t407-t7.*t10.*t163.*t365)+t579.*(t44.*t440+t27.*t43.*t442)-q2d1.*t411+q2d3.*t588+q2d4.*t665-q2d5.*t709+t200.*t360+q2d2.*(t10.*t303.^2+t10.*t404.^2+t256.^2)+qd5.*(t256.*t270+t200.*t383-t10.*t303.*(t161+t162-t194-t195)-t10.*(t299+t300).*(-t243+t305+t306)+t10.*t163.*t234+t10.*t334.*t390)+qd6.*(t200.*t329-t256.*t334-t10.*t200.*(-t243+t305+t306)+t10.*t256.*t334)-qd2.*(t256.*t360-t10.*t303.*t320+t10.*t324.*t329)+t10.*t598.*(t43.*t440-t27.*t44.*t442)-t10.*t163.*t320-t10.*t324.*t334-t10.*t436.*t600+t10.*t33.*t402.*t442,t585+t606+qd5.*(t200.*t549+t248.*(t269-t43.*(t143+t144-t249-t250))+t10.*t241.*(t143+t144-t249-t250)+t10.*t295.*(t299+t300)-t10.*t165.*t293+t10.*t334.*t563)-t579.*(t44.*t594+t27.*t43.*t596)-qd6.*(t200.*t295+t248.*t334-t10.*t200.*t295-t10.*t248.*t334)+q2d2.*t588-q2d1.*t591+q2d4.*t672-q2d5.*t711+t200.*t264+q2d3.*(t10.*t293.^2+t10.*t295.^2+t248.^2)-qd4.*(t248.*t344+t200.*t547-t10.*t293.*t314-t10.*t295.*t317+t10.*t334.*t560+t2.*t7.*t8.*t10.*t163.*(5.71e2./1.0e3))+qd3.*(-t248.*t264+t10.*t287.*t293+t10.*t292.*t295)+qd2.*(-t248.*t360+t10.*t293.*t320+t10.*t295.*t324)-t10.*t598.*(t43.*t594-t27.*t44.*t596)-t10.*t163.*t287-t10.*t292.*t334-t10.*t600.*t602-t10.*t33.*t402.*t596,t585+t606-t579.*(t44.*t429+t27.*t43.*t673)-qd6.*(t200.*t309+t266.*t334-t10.*t200.*t309-t10.*t266.*t334)+q2d2.*t665-q2d1.*t668+q2d3.*t672-q2d5.*t713+t200.*t344+qd5.*(t200.*t645+t266.*(t269-t43.*(t143+t144-t249-t250))+t10.*t27.*(t143+t144-t249-t250).*(3.0./2.5e1)-t7.*t10.*t165.*(3.0./2.5e1)+t10.*t334.*t652+t10.*(t299+t300).*(t243-t330))+qd3.*(-t264.*t266+t7.*t10.*t287.*(3.0./2.5e1)+t10.*t292.*t309)+qd4.*(-t266.*t344+t7.*t10.*t314.*(3.0./2.5e1)+t10.*t309.*t317)+qd2.*(-t266.*t360+t7.*t10.*t320.*(3.0./2.5e1)+t10.*t309.*t324)+q2d4.*(t10.*t33.*(9.0./6.25e2)+t10.*t662.^2+t266.^2)-t10.*t598.*(t43.*t429-t27.*t44.*t673)-t10.*t163.*t314-t10.*t317.*t334-t10.*t600.*t602-t10.*t33.*t402.*t673,q2d1.*(t714+t715)+qd3.*(t43.*t264.*(1.17e2./1.0e3)+t10.*t44.*t292.*(1.17e2./1.0e3))-qd5.*(t43.*t270.*(1.17e2./1.0e3)-t10.*t44.*t301.*(1.17e2./1.0e3))+qd4.*(t43.*t344.*(1.17e2./1.0e3)+t10.*t44.*t317.*(1.17e2./1.0e3))+qd2.*(t43.*t360.*(1.17e2./1.0e3)+t10.*t44.*t324.*(1.17e2./1.0e3))-qd6.*(t44.*t200.*(1.17e2./1.0e3)-t43.*t334.*(1.17e2./1.0e3)-t10.*t44.*t200.*(1.17e2./1.0e3)+t10.*t43.*t334.*(1.17e2./1.0e3))-q2d2.*t709-q2d3.*t711-q2d4.*t713-t200.*t270+q2d5.*(t357.*1.3689e-2+t10.*t355.*1.3689e-2)+t43.*t716.*(t577+t578)+t10.*(t161+t162-t194-t195).*(t143+t144-t249-t250)-t10.*t301.*t334+t7.*t10.*t402.*t430-t7.*t43.*t402.*t583-t10.*t27.*t402.*t605-t10.*t44.*t598.*t716-t7.*t10.*t44.*t402.*t600,t600.*(t577+t578)+t200.*t334-t583.*t598-t10.*t200.*t334-t10.*t579.*t600+t10.*t583.*(t597-t674)],[6,10]);


classdef dme_branch < mp.dm_element
%MP.DME_BRANCH  MATPOWER data model class for branch data

%   MATPOWER
%   Copyright (c) 2020-2022, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MATPOWER.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://matpower.org for more info.

    properties
        fbus    %% bus index vector for "from" port (port 1) (all branches)
        tbus    %% bus index vector for "to" port (port 2) (all branches)
        r       %% series resistance (p.u.) for branches that are on
        x       %% series reactance (p.u.) for branches that are on
        g_fr    %% shunt conductance (p.u.) at "from" end for branches that are on
        g_to    %% shunt conductance (p.u.) at "to" end for branches that are on
        b_fr    %% shunt susceptance (p.u.) at "from" end for branches that are on
        b_to    %% shunt susceptance (p.u.) at "to" end for branches that are on
        tm      %% transformer off-nominal turns ratio for branches that are on
        ta      %% xformer phase-shift angle (radians) for branches that are on
        rate_a  %% long term flow limit (p.u.) for branches that are on
    end     %% properties

    methods
        function name = name(obj)
            name = 'branch';
        end

        function label = label(obj)
            label = 'Branch';
        end

        function label = labels(obj)
            label = 'Branches';
        end

        function name = cxn_type(obj)
            name = 'bus';
        end

        function name = cxn_idx_prop(obj)
            name = {'fbus', 'tbus'};
        end

        function names = main_table_var_names(obj)
            names = horzcat( main_table_var_names@mp.dm_element(obj), ...
                {'bus_fr', 'bus_to', 'r', 'x', 'g_fr', 'b_fr', ...
                'g_to', 'b_to', 'sm_ub_a', 'sm_ub_b', 'sm_ub_c', ...
                'cm_ub_a', 'cm_ub_b', 'cm_ub_c', 'vad_lb', 'vad_ub', ...
                'tm', 'ta', ... %% remove these when we separate out xformers
                'pl_fr', 'ql_fr', 'pl_to', 'ql_to'} );
        end

        function vars = export_vars(obj)
            vars = {'pl_fr', 'ql_fr', 'pl_to', 'ql_to'};
        end

        function obj = initialize(obj, dm)
            initialize@mp.dm_element(obj, dm);  %% call parent

            %% get bus mapping info
            b2i = dm.elements.bus.ID2i;     %% bus num to idx mapping

            %% set bus index vectors for port connectivity
            obj.fbus = b2i(obj.tab.bus_fr);
            obj.tbus = b2i(obj.tab.bus_to);
        end

        function obj = update_status(obj, dm)
            %% get bus status info
            bs = dm.elements.bus.tab.status;    %% bus status

            %% update status of branches connected to isolated/offline buses
            obj.tab.status = obj.tab.status & bs(obj.fbus) & ...
                                              bs(obj.tbus);

            %% call parent to fill in on/off
            update_status@mp.dm_element(obj, dm);
        end

        function obj = build_params(obj, dm)
            obj.r  = obj.tab.r(obj.on);
            obj.x  = obj.tab.x(obj.on);
            obj.g_fr  = obj.tab.g_fr(obj.on);
            obj.b_fr  = obj.tab.b_fr(obj.on);
            obj.g_to  = obj.tab.g_to(obj.on);
            obj.b_to  = obj.tab.b_to(obj.on);
            obj.tm = obj.tab.tm(obj.on);
            obj.ta = obj.tab.ta(obj.on) * pi/180;
            obj.rate_a = obj.tab.sm_ub_a(obj.on) / dm.base_mva;
        end

        function obj = pp_data_cnt(obj, dm, rows, out_e, mpopt, fd, pp_args)
            %% call parent
            pp_data_cnt@mp.dm_element(obj, dm, rows, out_e, mpopt, fd, pp_args);

            num_xf = length(find(obj.tab.tm));
            num_xf_on = length(find(obj.tab.tm(obj.on)));
            num_ln = obj.nr - num_xf;
            num_ln_on = obj.n - num_xf_on;
            ln = sprintf('%d', num_ln);
            if num_ln == num_ln_on
                ln_on = ln;
                ln_off = '-';
            else
                ln_on = sprintf('%d', num_ln_on);;
                ln_off = sprintf('%d', num_ln - num_ln_on);
            end
            xf = sprintf('%d', num_xf);
            if num_xf == num_xf_on
                xf_on = xf;
                xf_off = '-';
            else
                xf_on = sprintf('%d', num_xf_on);;
                xf_off = sprintf('%d', num_xf - num_xf_on);
            end

            %% print line, transformer counts
            fprintf(fd, '  %-20s%7s %7s %7s\n', '  Lines', ln_on, ln_off, ln);
            fprintf(fd, '  %-20s%7s %7s %7s\n', '  Transformers', xf_on, xf_off, xf);
        end

        function TorF = pp_have_section_sum(obj, mpopt, pp_args)
            TorF = true;
        end

        function obj = pp_data_sum(obj, dm, rows, out_e, mpopt, fd, pp_args)
            %% call parent
            pp_data_sum@mp.dm_element(obj, dm, rows, out_e, mpopt, fd, pp_args);

            %% print branch summary
            fprintf(fd, '  %-29s  %12.2f MW', 'Total branch losses', ...
                sum(obj.tab.pl_fr(obj.on)) + sum(obj.tab.pl_to(obj.on)) );
            if mpopt.model(1) ~= 'D'    %% AC model
                fprintf(fd, ' %12.2f MVAr', ...
                    sum(obj.tab.ql_fr(obj.on)) + sum(obj.tab.ql_to(obj.on)) );
            end
            fprintf(fd, '\n');
        end

        function h = pp_get_headers_det(obj, dm, out_e, mpopt, pp_args)
            h = [ pp_get_headers_det@mp.dm_element(obj, dm, out_e, mpopt, pp_args) ...
                {   ' Branch     From       To             From Bus Injection   To Bus Injection', ...
                    '   ID      Bus ID    Bus ID   Status   P (MW)   Q (MVAr)   P (MW)   Q (MVAr)', ...
                    '--------  --------  --------  ------  --------  --------  --------  --------' } ];
            %%       1234567 123456789 123456789 -----1 1234567.90 123456.89 123456.89 123456.89
        end

        function TorF = pp_have_section_det(obj, mpopt, pp_args)
            TorF = true;
        end

        function str = pp_data_row_det(obj, dm, k, out_e, mpopt, fd, pp_args)
            str = sprintf('%7d %9d %9d %6d %10.2f %9.2f %9.2f %9.2f', ...
                obj.tab.uid(k), obj.tab.bus_fr(k), obj.tab.bus_to(k), ...
                obj.tab.status(k), ...
                obj.tab.pl_fr(k), obj.tab.ql_fr(k), ...
                obj.tab.pl_to(k), obj.tab.ql_to(k) );
        end
    end     %% methods
end         %% classdef


function [] = showroom(varargin)
% SHOWROOM - Revolves a 3D plot showroom-style.
%
% This function was especially designed to provide for those moments when you just created a
% stunning 3D plot, and feel the need to sit back and admire the result for a moment. The
% animation mimicks a cliché revolving automobile showroom, hence the name.
%
% Syntax:
%   SHOWROOM           Revolves the current plot at the default speed for one revolution
%   SHOWROOM(N)        Revolves N times
%   SHOWROOM(N, V)     Revolves N times with angular speed V (deg / sec)
%
% - By default one revolution (N = 1) is performed at 30 degrees per second (V = 30).
% - When no figure is active at the moment, a standard peaks plot is generated.
% - A button press or click on the figure terminates the animation prematurely.
% - For counterclockwise movement, enter a negative value for V.
% - For 2D views and views with elevations close to 90 degrees this function happens to
%   cause a psychedelic flip-effect.
%
% Jasper Menger (j.t.menger@alumnus.utwente.nl), November 2005

% Get the user inputs
N = 1;    % Default number of revolutions
V = 30;   % Default revolution speed
if numel(varargin) > 1
    % Both speed and revolutions provided
    N = round(varargin{1});
    V = varargin{2};
elseif numel(varargin) == 1
    % Only revolutions provided
    N = varargin{1};
end

% Create a peaks plot when no figure is active
I_fig = get(0,'CurrentFigure');
if not(numel(I_fig))
    figure;
    colormap('winter');
    hold on; grid on;
    surf(peaks);
    axis vis3d;
    view(3);
else
    figure(I_fig);
end

% Current view
drawnow;
[Az, El] = view;
El       = ceil(El);
% Start and end angle
Az_start = Az;
Az_stop  = Az_start + sign(V) * N * 360;
% First guess of plotting time
dT = 0.01;

% Enable user termination
click_ax_old  = get(gca, 'ButtonDownFcn');
click_fg_old  = get(gca, 'ButtonDownFcn');
press_fg_old  = get(gcf, 'KeyPressFcn');
userfield_old = get(gcf, 'UserData');
command_new   = 'set(gcf, ''UserData'', true)';
set(gcf, 'UserData'     , false);
set(gca, 'ButtonDownFcn', command_new);
set(gcf, 'ButtonDownFcn', command_new);
set(gcf, 'KeyPressFcn'  , command_new);

% Animation loop!
Az    = Az_start;
abort = false;
while (abort == false) && abs(Az - Az_start) < abs(Az_stop - Az_start)
    % Check for abort command
    abort = get(gcf, 'UserData');
    % Start stopwatch
    tic;
    % Increase the angle
    dAz = V * dT;
    Az  = Az + dAz;
    % Change the view
    view([Az, El]);
    drawnow;
    % Stop stopwatch
    dT = toc;
end

% Restore old values
view([Az_start, El]);
set(gca, 'ButtonDownFcn', click_ax_old);
set(gcf, 'ButtonDownFcn', click_fg_old);
set(gcf, 'KeyPressFcn'  , press_fg_old);
set(gcf, 'UserData'     , userfield_old);


%%% INPUT THE IMAGE FILE NAME:

graphout = 0;

if ~exist('fc')|~exist('cc')|~exist('kc')|~exist('alpha_c'),
   fprintf(1,'No intrinsic camera parameters available. Maybe, need to load Calib_Results.mat\n');
   return;
end;

KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2) ; 0 0 1];

disp('Program that undistorts a whole sequence of images (works with bmp only so far... needs some debugging)');
disp('The intrinsic camera parameters are assumed to be known (previously computed)');
disp('After undistortion, the intrinsic parameters fc, cc, alpha_c remain unchanged. The distortion coefficient vector kc is zero');

dir;

fprintf(1,'\n');

seq_name = input('Basename of sequence images (without number nor suffix): ','s');

format_image_seq = '0';

while format_image_seq == '0',
    format_image_seq =  input('Image format: ([]=''r''=''ras'', ''b''=''bmp'', ''t''=''tif'', ''p''=''pgm'', ''j''=''jpg'', ''m''=''ppm'') ','s');
    if isempty(format_image_seq),
        format_image_seq = 'ras';
    else
        if lower(format_image_seq(1)) == 'm',
            format_image_seq = 'ppm';
        else
            if lower(format_image_seq(1)) == 'b',
                format_image_seq = 'bmp';
            else
                if lower(format_image_seq(1)) == 't',
                    format_image_seq = 'tif';
                else
                    if lower(format_image_seq(1)) == 'p',
                        format_image_seq = 'pgm';
                    else
                        if lower(format_image_seq(1)) == 'j',
                            format_image_seq = 'jpg';
                        else
                            if lower(format_image_seq(1)) == 'r',
                                format_image_seq = 'ras';
                            else
                                disp('Invalid image format');
                                format_image_seq = '0'; % Ask for format once again
                            end;
                        end;
                    end;
                end;
            end;
        end;
    end;
end;

ima_sequence = dir( [ seq_name '*.' format_image_seq]);

if isempty(ima_sequence),
    fprintf(1,'No image found\n');
    return;
end;

ima_name = ima_sequence(1).name;
if format_image_seq(1) == 'p',
    if format_image_seq(2) == 'p',
        I = double(loadppm(ima_name));
    else
        I = double(loadpgm(ima_name));
    end;
else
    if format_image_seq(1) == 'r',
        I = readras(ima_name);
    else
        I = double(imread(ima_name));
    end;
end;

[ny,nx,nc] = size(I);

% Pre-compute the necessary indices and blending coefficients to enable quick rectification:
[Irec_junk,ind_new,ind_1,ind_2,ind_3,ind_4,a1,a2,a3,a4] = rect_index(zeros(ny,nx),eye(3),fc,cc,kc,alpha_c,KK);

n_seq = length(ima_sequence);

for kk = 1:n_seq,

    ima_name = ima_sequence(kk).name;

            fprintf(1,'Loading original image %s...',ima_name);

    %%% READ IN IMAGE:

    if format_image_seq(1) == 'p',
        if format_image_seq(2) == 'p',
            I = double(loadppm(ima_name));
        else
            I = double(loadpgm(ima_name));
        end;
    else
        if format_image_seq(1) == 'r',
            I = readras(ima_name);
        else
            I = double(imread(ima_name));
        end;
    end;

    [ny,nx,nc] = size(I);

    if graphout,
        figure(2);
        image(uint8(I));
        drawnow;
    end;

    I2 = zeros(ny,nx,nc);

    for ii = 1:nc,

        Iii = I(:,:,ii);
        I2ii = zeros(ny,nx);

        I2ii(ind_new) = uint8(a1 .* Iii(ind_1) + a2 .* Iii(ind_2) + a3 .* Iii(ind_3) + a4 .* Iii(ind_4));

        I2(:,:,ii) = I2ii;

    end;

    I2 = uint8(I2);

    if graphout,
        figure(3);
        image(I2);
        drawnow;
    end;

    ima_name2 = ['undist_' ima_name];

    fprintf(1,'Saving undistorted image under %s...\n',ima_name2);

    if format_image_seq(1) == 'p',
        if format_image_seq(2) == 'p',
            saveppm(ima_name2,I2);
        else
            savepgm(ima_name2,I2);
        end;
    else
        if format_image_seq(1) == 'r',
            writeras(ima_name2,I2,gray(256));
        else
            imwrite(I2,ima_name2,format_image_seq);
        end;
    end;

end;


function Z = separate_freq(Y)
	if iscell(Y)
		Z = {};

		for m = 0:length(Y)-1
			Z{m+1} = separate_freq(Y{m+1});
		end

		return;
	end

	Z.signal = {};
	Z.meta = struct();

	r = 1;
	for k = 1:length(Y.signal)
		if ~iscell(Y.signal{k})
			% because matlab is stupid and ignores last dim if 1
			sz_orig = size(Y.signal{k});
			if numel(sz_orig) == 2
				sz_orig(3) = 1;
			end
		else
			sz_orig = [length(Y.signal{k}) 0 size(Y.signal{k}{1},2)];
		end

		for l = 1:sz_orig(1)
			if ~iscell(Y.signal{k})
				nsignal = Y.signal{k}(l,:);
			else
				nsignal = Y.signal{k}{l};
			end

			nsignal = reshape(nsignal,[numel(nsignal)/sz_orig(3) sz_orig(3)]);

			Z.signal{r} = nsignal;

			r = r+1;
		end
	end

	[temp,I] = sortrows(Y.meta.j(1:size(Y.meta.j,1),:).');

	Z.signal = Z.signal(I);

	field_names = fieldnames(Y.meta);
	for n = 1:length(field_names)
		src_value = getfield(Y.meta,field_names{n});
		Z.meta = setfield(Z.meta,field_names{n},src_value(:,I));
	end

	Z.signal = cellfun(@(x)(permute(x,[1 3 2])), Z.signal, ...
		'UniformOutput', false);
end


% Copyright (C) 2018  Symeon Symeonidis, Stefanos Tsantilas, Stelios Mitilineos
% simos421@gmail.com, steftsantilas@gmail.com, smitil@gmail.com
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.

function Cstblood(mws)

material = invoke(mws,'material');
invoke(material,'Reset');
invoke(material,'Name','Blood'); %Blood
invoke(material,'FrqType','all');
invoke(material,'Type','Normal');
invoke(material,'SetMaterialUnit','GHz','mm');
invoke(material,'Epsilon','1');
invoke(material,'Mue','1');
invoke(material,'Sigma','0.0');
invoke(material,'TanD','0.0');
invoke(material,'TanDFreq','0.0');
invoke(material,'TanDGiven','False');
invoke(material,'TanDModel','ConstSigma');
invoke(material,'ConstTanDModelOrderEps','1');
invoke(material,'ReferenceCoordSystem','Global');
invoke(material,'CoordSystemType','Cartesian');
invoke(material,'SigmaM','0.0');
invoke(material,'TanDM','0.0');
invoke(material,'TanDMFreq','0.0');
invoke(material,'TanDMGiven','False');
invoke(material,'TanDMModel','ConstSigma');
invoke(material,'ConstTanDModelOrderMue','1');
invoke(material,'DispModelEps','None');
invoke(material,'DispModelMue','None');
invoke(material,'DispersiveFittingSchemeEps','Nth Order');
invoke(material,'MaximalOrderNthModelFitEps','3');
invoke(material,'ErrorLimitNthModelFitEps','0.01');
invoke(material,'DispersiveFittingSchemeMue','Nth Order');
invoke(material,'AddDispersionFittingValueEps','0.1', '76.8182454443825', '221.641134494173', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.2', '68.4738432550289', '115.059990632458', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.3', '65.6502510183341', '78.8589515742776', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.4', '64.1828321718796', '60.6489945539656', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.422222222222222', '63.9438618885212', '57.7730434712015', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.5', '63.257247554219', '49.7333267655039', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.6', '62.6027643145963', '42.5001241802731', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.7', '62.1030699559808', '37.3865932409505', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.744444444444444', '61.9142639813766', '35.5731389635495', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.8', '61.6997719763589', '33.6053481035262', '1.0');
invoke(material,'AddDispersionFittingValueEps','0.9', '61.3602871921475', '30.7161644656542', '1.0');
invoke(material,'AddDispersionFittingValueEps','1.06666666666667', '60.8862780821185', '27.206610582395', '1.0');
invoke(material,'AddDispersionFittingValueEps','1.38888888888889', '60.1510807057843', '23.0953297030954', '1.0');
invoke(material,'AddDispersionFittingValueEps','1.71111111111111', '59.5320763343363', '20.8376504484609', '1.0');
invoke(material,'AddDispersionFittingValueEps','2.03333333333333', '58.9648498262843', '19.545237054196', '1.0');
invoke(material,'AddDispersionFittingValueEps','2.35555555555556', '58.4208755792015', '18.8129113591261', '1.0');
invoke(material,'AddDispersionFittingValueEps','2.67777777777778', '57.8860468115685', '18.4294319222186', '1.0');
invoke(material,'AddDispersionFittingValueEps','3', '57.3529593274927', '18.4294319222186', '1.0');
invoke(material,'UseGeneralDispersionEps','True');
invoke(material,'UseGeneralDispersionMue','False');
invoke(material,'NLAnisotropy','False');
invoke(material,'NLAStackingFactor','1');
invoke(material,'NLADirectionX','1');
invoke(material,'NLADirectionY','0');
invoke(material,'NLADirectionZ','0');
invoke(material,'Rho','1060');
invoke(material,'ThermalType','Normal');
invoke(material,'ThermalConductivity','0.51');
invoke(material,'HeatCapacity','3.824');
invoke(material,'MetabolicRate','0');
invoke(material,'BloodFlow','1e+006');
invoke(material,'VoxelConvection','0');
invoke(material,'MechanicsType','Unused');
invoke(material,'Colour','1','0','0');
invoke(material,'Wireframe','False');
invoke(material,'Reflection','False');
invoke(material,'Allowoutline','False');
invoke(material,'Transparentoutline','False');
invoke(material,'Transparency','0');
invoke(material,'Create');
release(material);
end


function coordsys=m_coord(coordsys)
% M_COORD Initializes the coordinate system for varous conversions.
%
%   M_COORD('set') tells you the current coordinate system
%   M_COORD('get') gives you the current possibilities
%   M_COORD(SYSTEM) sets the coordinate system to SYSTEM.
%
%   Currently the available coordinate systems are:
%              'geographic' (usual lat/long)
%              'geomagnetic' (referenced to magnetic poles)

% Rich Pawlowicz (rich@ocgy.ubc.ca) nOV/2001
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.

global MAP_COORDS

if nargin==0, coordsys='usage'; end

coordsys=lower(coordsys);

coords=mc_coords('name');

switch coordsys

   case 'get'
      disp(' ');
      disp('Available coordinate systems are:');
      for k=1:length(coords.name)
        disp(['     ' coords.name{k}]);
      end

   case 'set'
      if isempty(MAP_COORDS)
         disp('No coordinate system initialized');
         m_coord('usage');
      else
         if nargout==0
           disp(MAP_COORDS.name);
         else
	       coordsys=MAP_COORDS;
         end
      end

   case 'usage'
      disp(' ');
      disp('Possible calling options are:');
      disp('  ''usage''                    - this list');
      disp('  ''set''                      - list of coordinate systems');
      disp('  ''get''                      - get current coordinate (if defined)');
      disp('  ''system''                   - initialize coordinate system\n');

   otherwise
     k=strcmp(coordsys,lower(coords.name));
     MAP_COORDS=mc_coords('parameters',coords.name{k});

end
    % Check to see if a non-lat/long coordinate system is being used.


clear
close all
clc;

addpath('./local_functions_demo1')
addpath '../libs/exportFig/';
addpath './fun4MeanShift';

path_to_matconvnet = '../libs/matconvnet-1.0-beta23_modifiedDagnn';
run(fullfile(path_to_matconvnet, 'matlab', 'vl_setupnn'));
addpath(genpath(fullfile('dependencies', 'matconvnet','examples')));

dataDir = './dataset';
%% read matconvnet model
% set GPU
flagSaveFig = true; % {true false} whether to store the result

h = 28;
w = 28;
panelSZ = round(64/2)*2;
stepSize = (panelSZ-h)/2;
posList = [1, stepSize, 2*stepSize];
posList = repmat(posList, 3, 1);
posList = cat(3, posList, posList');
posList = reshape(posList, [], 2);
N = size(posList,1)+1;
%% prepare imdb
imdb.path_to_dataset = './toydata_v3';

fileList = dir( fullfile(imdb.path_to_dataset, 'batch*mat') );
imdb.imgList = {fileList.name};
imdb.set = ones(1, length(imdb.imgList));

fileList = dir( fullfile(imdb.path_to_dataset, 'test*mat') );
imdb.imgList(end+1:end+length({fileList.name})) = {fileList.name};
imdb.set(end+1:end+length({fileList.name})) = 2;

imdb.sets.name = {'train', 'val'};

% meanValue = 0;
% for i = 1:sum(imdb.set==1)
%     if mod(i,100) == 0
%         fprintf('\t%d/%d\n', i, sum(imdb.set==1));
%     end
%     tmpMat = load(fullfile(imdb.path_to_dataset, imdb.imgList{i}));
%     meanValue = meanValue + mean(tmpMat.imgMat(:));
% end
% meanValue = meanValue / sum(imdb.set==1);

meanValue = reshape([0.0717, 0.0820, 0.0715], [1 1 3]);
imdb.meta.meanvalue = meanValue; % 0.0981
imdb.meta.className = {};
for i = 1:9
    imdb.meta.className{end+1} = int2str(i);
end
imdb.meta.className{end+1} = '10';
imdb.meta.className{end+1} = 'void';
imdb.meta.classNum = length(imdb.meta.className);
imdb.meta.height = panelSZ;
imdb.meta.width = panelSZ;

save('imdb_toydata_v3_from_mnist.mat', 'imdb')


function lung_image = PTKFillCoronalHoles(lung_image, is_right, reporting)
    % PTKFillCoronalHoles. Operates on each coronal slice, applying a closing
    % filter then filling interior holes
    %
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2012.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    reporting.ShowProgress('Filling holes');

    max_non_coronal_voxel_size = max(lung_image.VoxelSize(2:3));

    opening_size = 10/max_non_coronal_voxel_size;
    closing_size = 10/max_non_coronal_voxel_size;

    lung_image.AddBorder(10);
    for coronal_index = 11 : lung_image.ImageSize(1) - 10
        reporting.UpdateProgressStage(coronal_index - 11, lung_image.ImageSize(1) - 20);
        coronal_slice = lung_image.GetSlice(coronal_index, PTKImageOrientation.Coronal);
        if ~isempty(is_right)
            coronal_slice = OpenOrClose(coronal_slice, is_right, opening_size, closing_size, reporting);
        end
        coronal_slice = imclose(coronal_slice, strel('disk', round(closing_size)));
        coronal_slice = imfill(coronal_slice, 'holes');
        lung_image.ReplaceImageSlice(coronal_slice, coronal_index, PTKImageOrientation.Coronal);
    end
    lung_image.RemoveBorder(10);
    reporting.CompleteProgress;
end

function mask = OpenOrClose(mask, is_right, opening_size, closing_size, reporting)
    closed_image = imclose(mask, strel('disk', round(closing_size)));
    opened_image = imopen(mask, strel('disk', round(opening_size)));

    threshold = PTKImage(~(closed_image & opened_image));
    threshold.AddBorder(1);
    image_size = threshold.ImageSize;

    raw_image = zeros(image_size);
    if (is_right)
        raw_image(2, 2:end-1, 2) = 2;
        raw_image(end-1, 2:end-1, 2) = 1;
    else
        raw_image(2, 2:end-1, 2) = 1;
        raw_image(end-1, 2:end-1, 2) = 2;
    end
    raw_image(2:end-1, end-1, 2) = 2;
    raw_image(2:end-1, 2, 2) = 2;
    right_border_indices = threshold.LocalToGlobalIndices(find(raw_image == 1));
    other_border_indices = threshold.LocalToGlobalIndices(find(raw_image == 2));

    start_points = {right_border_indices, other_border_indices};

    reporting.PushProgress;
    regions = PTKMultipleRegionGrowing(threshold, start_points, reporting);
    reporting.PopProgress;

    regions.RemoveBorder(1);
    closed_region = (regions.RawImage == 1);
    mask(closed_region) = closed_image(closed_region);
    mask(~closed_region) = opened_image(~closed_region);
end


function p=rmReconParams(vw)

% this parameters is needed to load all raw-data
p.wData = 'all';

% convert data to percent BOLD
p.analysis.calcPC  = true;

% number of trends
p.analysis.nDCT = 4;

% stimulus parameters
ns = viewGet(vw,'nscans');
for n=1:ns
    p.stim(n).nFrames    = viewGet(vw,'numframes',n);
    p.stim(n).nUniqueRep = 1;
    p.stim(n).nDCT = p.analysis.nDCT;
end

% thresholds above which to use voxels
p.thresh.rmVarexp = 0.15; % goodness of fit of pRF model
p.thresh.stimTmap = 1.96; % goodness of fit of stimuli

% Number of events in a par file
p.numOfEvents = 37;


function channels = demCmu35VargplvmLoadChannels(Ytest, skel)
% DEMCMU35VARGPLVMLOADCHANNELS Given a dataset load the skeleton channels
%
% SEEALSO : demCmu35VargplvmAnimate, demCmu35gplvmVargplvm3.m
% VARGPLVM

% playSkel(Ytest, startInd)
    if nargin < 2 || isempty(skel)
        skel = acclaimReadSkel('35.asf');
        [tmpchan, skel] = acclaimLoadChannels('35_01.amc', skel);
    end

    %left indices
    xyzInd = [2];
    xyzDiffInd = [1 3];
    rotInd = [4 6];
    rotDiffInd = [5];
    generalInd = [7:38 41:47 49:50 53:59 61:62];
    startInd = 1;
    endInd = length(generalInd);
    channels(:, generalInd) = 180*Ytest(:, startInd:endInd)/pi;
    startInd = endInd + 1;
    endInd = endInd + length(xyzDiffInd);
    channels(:, xyzDiffInd) = cumsum(Ytest(:, startInd:endInd), 1);
    startInd = endInd + 1;
    endInd = endInd + length(xyzInd);
    channels(:, xyzInd) = Ytest(:, startInd:endInd);
    startInd = endInd + 1;
    endInd = endInd + length(xyzDiffInd);
    channels(:, xyzDiffInd) = cumsum(Ytest(:, startInd:endInd), 1);
    startInd = endInd + 1;
    endInd = endInd + length(rotInd);
    channels(:, rotInd) = asin(Ytest(:, startInd:endInd))*180/pi;
    channels(:, rotInd(end)) = channels(:, rotInd(end))+270;
    startInd = endInd + 1;
    endInd = endInd + length(rotDiffInd);
    channels(:, rotDiffInd) = 0;%cumsum(asin(Ytest(:, startInd:endInd)), 1))*180/pi;
    % skelPlayData(skel, channels, 1/25);


function [fibercurvature] = dtiComputeFiberCurvature(fg)
%
% [fibercurvature] = dtiComputeFiberCurvature(fg)
%
% Compute the curvature of individual fibers and make a cell-array output
% of curvatures in each node.
%
% % INPUTS:
% fibers   = A fiber group.
%
% % OUTPUTS:
% fibercurvature = A cell array of fiber curvarture.
%
% This code is an updated version of dtiComputeFiberGroupCurvatures.m
%
% (C) Hiromasa Takemura, Stanford VISTA team, 2014

if notDefined('fg'), error('Fiber group required'); end

nfibers=size(fg.fibers, 1);

for i=1:nfibers

    numNodes(i) =size(fg.fibers{i}, 2);

    %Compute curvature representations
    fibercurvature{i}=dtiFiberCurvature(fg.fibers{i});
end


function imStack=analyze2mrLoadRetTSeries(inFileRoot,outFileRoot,nVols,firstVolIndex,doRotate,scaleFact,flipFlag)
% function imStack=analyze2mrLoadRetTSeries(inFileRoot,outFileRoot,nVols,firstVolIndex,doRotate,scaleFact,flipFlag)
% Converts from analyze functional image data to mrLoadRet TSeries format
% Analyze functional data are stored as N individual volumes, each with S slices
% mrLoadRet has S individual files with N acquisitions in each.
% the doRotate param allows to you roate the functional data by doRotate*90 degrees
% Try to read the the first volume to see if it's there and get the dimensions of all the rest

% TODO: 1:  Rotate these so that they are in the same orientation as the anatomicals
% (Maybe make this a flag)
% 2: Why can't we see the anatomicals? Claims not to be able to read from /Raw/Anatomy/Inplane
% 3: Why can't we load in the corAnal? We do this by hand perfectly well but it won't do it from mrLoadRet
% 4: Do we want to resample the functional data? Or is this the moment to go to mrLoadRet3?
%

if (~exist('firstVolIndex','var'))
    firstVolIndex=1;
end

if (~exist('scaleFact','var'))
    scaleFact=[1 1]; % No interpolation. (Scaling=1)
end

if (length(scaleFact)~=2) % If we just get a scalar for the scale factor, assume that it applies in both dimensions
    scaleFact=repmat(scaleFact(1),2);
end

if (~exist('doRotate','var'))
    doRotate=1; % This is on by default. Rotates 1*90 degrees
end

if (~exist('flipFlag','var'))
    flipFlag=1; % This is on by default. Flips up/down after rotation
end
suffix=sprintf('%03d',firstVolIndex);

fileName=[inFileRoot,suffix,'.hdr'];

V=spm_vol(fileName);
im=spm_read_vols(V);

[y,x,nSlices]=size(im);
fprintf('Read in a volume of size %d, %d, %d',y,x,nSlices);

% Pre-allocate memory: This is a monster and will fail on many machines.
fprintf('\nTrying to allocate an array with %d elements...\n',y*x*nSlices*nVols);

if (mod(doRotate,2)) % When we rotate by 180 degrees the x and y dimensions remain unchanged
    funcVol=zeros(y,x,nSlices,nVols);
else
    funcVol=zeros(x,y,nSlices,nVols);
end
 fprintf('Rotating by %d x 90, flipFlag=%d',doRotate,flipFlag);

for t=0:(nVols-1)
    thisImIndex=t+firstVolIndex;
    suffix=sprintf('%03d',thisImIndex);
    fileName=[inFileRoot,suffix];
    V=spm_vol(fileName);
    im=spm_read_vols(V);

    % Do the rotation and scaling

    if (mod(doRotate,2)) % When we rotate by 180 degrees the x and y dimensions remain unchanged
        im2=zeros(y,x,nSlices);
    else
        im2=zeros(x,y,nSlices);
    end
    fprintf('\nVol=%d',thisImIndex);

    for thisSlice=1:nSlices
        imSlice=squeeze(im(:,:,thisSlice));
    %s    imSlice=imresize(imSlice,[scaleFact(1)*y,scaleFact(2)*x],'nearest');

        im2(:,:,thisSlice)=rot90(imSlice,doRotate);
        if (flipFlag)

            im2(:,:,thisSlice)=flipud(im2(:,:,thisSlice));
        end

    end % next imSlice

    funcVol(:,:,:,t+1)=im2;
    %fprintf('.');

end
size(funcVol);
[y, x, nSlices, nVols]=size(funcVol);

% Now write them out in a different format
fprintf('\nDone reading data: Writing now...\n');
for t=1:nSlices
    suffix=int2str(t);
    fileName=[outFileRoot,suffix,'.dat'];
    tSeries=squeeze(funcVol(:,:,t,:));
    [a,b,c]=size(tSeries);
    fprintf('\nSize before: %d by %d by %d vols ',a,b,c);

    tSeries=squeeze(shiftdim(tSeries,2));
    [a,b,c]=size(tSeries);
    fprintf('\nSize after:%d slices by %d by %d',a,b*scaleFact(1),c*scaleFact(2));

    savetSeriesDat3(fileName,tSeries,y,x,scaleFact);

    fprintf('_');
end
fprintf('\nDone\n');


function candidates = compute_mcg_cands(params, rf, n_cands, mcg_cache_obj, D, RD)
% function candidates = compute_mcg_cands(params, rf, n_cands, mcg_cache_obj)

% ------------------------------------------------------------------------
%  Copyright (C)
%  Universitat Politecnica de Catalunya BarcelonaTech (UPC) - Spain
%  University of California Berkeley (UCB) - USA
%
%  Jordi Pont-Tuset <jordi.pont@upc.edu>
%  Pablo Arbelaez <arbelaez@berkeley.edu>
%  June 2014
% ------------------------------------------------------------------------
% This file is part of the MCG package presented in:
%    Arbelaez P, Pont-Tuset J, Barron J, Marques F, Malik J,
%    "Multiscale Combinatorial Grouping,"
%    Computer Vision and Pattern Recognition (CVPR) 2014.
% Please consider citing the paper if you use this code.
% ------------------------------------------------------------------------

  n_hiers = length(params.hier_dirs);

  % Load trained random forest
  if(isstr(rf)), rf = loadvar(rf,'rf'); end

  % Load Pareto parameters
  if(isstr(n_cands)), n_cands = loadvar(n_cands,'n_cands'); end

  % Number of regions per candidate
  assert(n_hiers==size(n_cands,2));

  f_lp = mcg_cache_obj.f_lp; f_ms = full(mcg_cache_obj.f_ms); feats = mcg_cache_obj.feats;
  bboxes = mcg_cache_obj.bboxes; red_cands = mcg_cache_obj.red_cands;
  b_feats_intersections = mcg_cache_obj.b_feats_intersection;

  if(params.depth_features)
    tt = tic();
    sp = f_lp;
    sp2regC = cands2labels(red_cands, f_ms);
    sp2reg = false(max(sp(:)), length(sp2regC));
    for j = 1:length(sp2regC), sp2reg(sp2regC{j},j) = true; end
    D = double(D)./1000;
    C = params.camera_matrix;
    missingMask = RD == 0;
    fdepth = depthFeatures(sp, sp2reg, D, missingMask, C);
    feats = cat(2, feats, fdepth');
    fprintf('Time for depth features: %0.3f\n', toc(tt));
  end

  % Rank candidates
  class_scores = regRF_predict(feats,rf);
  [scores, ids] = sort(class_scores,'descend');
  red_cands = red_cands(ids,:);
  bboxes = bboxes(ids,:);
  if isrow(scores), scores = scores'; end

  % Max margin
  candidates=[];
  [new_ids, mm_scores] = mex_max_margin(red_cands-1,scores, full(b_feats_intersections), params.theta); %#ok<NASGU>
  cand_labels = red_cands(new_ids,:);
  candidates.scores = scores(new_ids);
  bboxes = bboxes(new_ids,:);

  % Change the coordinates of bboxes to be coherent with
  % other results from other sources (sel_search, etc.)
  candidates.bboxes = [bboxes(:,2) bboxes(:,1) bboxes(:,4) bboxes(:,3)];

  % Get the labels of leave regions that form each candidates
  candidates.superpixels = f_lp;
  candidates.labels = cands2labels(cand_labels,f_ms);
end


function [hpatch, cl] = cluster_image_sphere(cl, varargin)
% :Usage:
% ::
%
%    [hpatch, cl] = cluster_image_sphere(cl or [k x 3 list of mm coords], varargin)
%
% Images spheres at cluster centers
% Combine with addbrain and cluster_tools('connect3d')
% or cluster_nmdsfig_glassbrain to get 3-D plots of connected blobs
% or surface_cutaway
%
% :Optional Inputs:
%
%   {'color', 'colors'}, mycolor = varargin{i+1};
%
%   'radius', myradius = varargin{i+1};
%
% :Outputs:
%
%   patch handles, and new cl with sphere coordiates in XYZmm and XYZ
%
% :Example: Usage
% ::
%
%    function [hpatch, cl] = cluster_image_sphere(cl)
%
%    % With optional arguments:
%    [hpatch, cl] = cluster_image_sphere(cl, 'color', 'b', 'radius', 10)
%    [hpatch, cl] = cluster_image_sphere(cl, 'color', {'r' 'g' 'b' etc}, 'radius', 10)
%
% :Examples: Given an MNI coordinate, plot a sphere on a brain surface
% ::
%
%    my_mm_coord = [40, 46, 22]';
%    create_figure('surface')
%    cl = [];
%    cl.XYZmm = my_mm_coord;
%    cl.mm_center = my_mm_coord';
%    V = spm_vol(which('brainmask.nii'));
%    cl.M = V.mat;
%    [hpatch, cl] = cluster_image_sphere(cl, 'color', 'g', 'radius', 10)
%    p = addbrain;
%    set(p, 'FaceAlpha', 1);
%    axis image
%    view(135, 30); lighting gouraud; lightRestoreSingle; material dull;
%
% Example: Turn xyz mm coordinates into clusters and image them
% ::
%
%    my_mm_coord = [40 46 22; 50 26 40; 45 36 50; 60 12 0]
%    [hpatch, cl] = cluster_image_sphere(my_mm_coord, 'color', 'b', 'radius', 4);
%
% ..
%    Tor Wager, July 2007 (original version)
%
%    Programmers' notes:
%    Tor Wager, July 2007
%    updated April 2011 for flexible radius
%    updated 12/2012 for xyz to spheres
% ..

    mycolor = 'r';
    myradius = 8;

    for i = 1:length(varargin)
        if ischar(varargin{i})
            switch varargin{i}
                % reserved keywords
                case {'color', 'colors'}, mycolor = varargin{i+1}; varargin{i+1} = [];

                case 'radius', myradius = varargin{i+1};

                otherwise, warning(['Unknown input string option:' varargin{i}]);
            end
        end
    end

    if isempty(cl)
        hpatch = [];
        return
    end

    if isa(cl, 'region')
       cl = region2struct(cl);
    end

    % if cl is actually a series of xyz coordinates...
    if ~isstruct(cl)
        cl = create_cl_struct(cl);
    end

    if length(myradius) == 1
        myradius = repmat(myradius, 1, length(cl));
    end

    % get colors for each sphere
    if length(mycolor) == 1 || ~iscell(mycolor)
        mycolor = repmat({mycolor}, 1, length(cl));
    end

   %newcl = convert_cl_xyz_to_sphere(cl(1), myradius);

    for i = 1:length(cl)

        newcl(i) = convert_cl_xyz_to_sphere(cl(i), myradius(i));

        hpatch(i) = image_isosurface(newcl(i), mycolor{i});

    end

    cl = newcl;

end

function cl = create_cl_struct(xyz)

V = spm_vol(which('brainmask.nii'));
if size(xyz, 2) ~= 3, xyz = xyz'; end
if size(xyz, 2) ~= 3, error('Bad input xyz coordinates?'); end

cl = [];
for i = 1:size(xyz, 1)
    my_mm_coord = xyz(i, :);
    cl(i).XYZmm = my_mm_coord';
    cl(i).mm_center = my_mm_coord;
    cl(i).M = V.mat;

    cl(i).voxSize = diag(cl(i).M(1:3, 1:3))';
end

end

function cl = convert_cl_xyz_to_sphere(cl, myradius)

    newXYZmm = round(points_in_sphere(cl(1).mm_center, myradius)');

    cl(1).Z = ones(1, size(newXYZmm,2));
    cl(1).XYZmm = newXYZmm;
    cl(1).XYZ = mm2voxel(cl(1).XYZmm, cl(1).M, 1)';
    cl(1).numVox = size(cl(1).XYZ, 2);

end

function hpatch = image_isosurface(cl, mycolor)

    % controls padding to make sure we cover whole area
    padval = 1;

    % controls smoothness, etc.
    mythresh = .5;
    mysmoothbox = 3;
    mygaussstd = 1;

    mm_coords = cl(1).XYZmm;

    xyzmin = min(mm_coords') - padval;     % minus/plus for padding
    xyzmax = max(mm_coords') + padval;

    [X, Y, Z] = meshgrid(xyzmin(1):xyzmax(1), xyzmin(2):xyzmax(2), xyzmin(3):xyzmax(3));

    % construct volume data for area to image

    xvox = mm_coords(1,:) - xyzmin(1) + 1;
    yvox = mm_coords(2,:) - xyzmin(2) + 1;
    zvox = mm_coords(3,:) - xyzmin(3) + 1;

    V = zeros(size(X));

    for i = 1:size(xvox, 2)
        V(xvox(i), yvox(i), zvox(i)) = cl(1).Z(i);
    end

    % not needed if we have all mm points in sphere
    %VI = interp3(cl(1).XYZmm(1,:), cl(1).XYZmm(2,:), cl(1).XYZmm(3,:), cl(1).Z, X, Y, Z);

    V = smooth3(V, 'gaussian', mysmoothbox, mygaussstd);
    FV = isosurface(X,Y,Z,V, mythresh);

    hpatch = patch(FV);
    set(hpatch, 'EdgeColor', 'none', 'FaceColor', mycolor);

    drawnow

    %lighting gouraud

end


C= 2;
T= 10000;
cnt= struct('fs', 100);
cnt.x= 50*randn(T, C);
cnt.clab= str_cprintf('Ch%d', 1:C);

M= 100;
mrk= struct; %('fs', cnt.fs);
mrk.time= round(linspace(0, T/cnt.fs*1000, M+2));
mrk.time([1 end])= [];
%mrk.desc= str_cprintf('S%3d', ceil(rand(1,M)*10))';
mrk.desc= ceil(rand(1,M)*10);

% --- Setup a very simple system for (simulated) online processing
bbci= struct;
bbci.source.acquire_fcn= @bbci_acquire_offline;
bbci.source.acquire_param= {cnt, mrk};
bbci.source.log.output= 'screen';
data= bbci_recordSignals(bbci, '/tmp/rec_test');

[cnt_re, mrk_re]= file_readBV(data.source.record.filename);

isequal(cnt.clab, cnt_re.clab)
max(abs(cnt.x(:)-cnt_re.x(:)))

isequal(mrk.pos, mrk_re.pos(2:end))
isequal(mrk.desc, mrk_re.desc(2:end))


randomseed(0);
x = randgamma(repmat(3,1,3));
randomseed(0);
x2 = randgamma(repmat(3,1,3));
assert(all(x == x2));
seed = randomseed;
x = randgamma(repmat(3,1,3));
randomseed([4 5 6]);
randomseed(seed);
x2 = randgamma(repmat(3,1,3));
assert(all(x == x2));
fprintf('randomseed test passed.\n');


function [yt Xt Xbart]=stvoltmat(Y,X,n,T)

% recover yt
yt=reshape(Y',[n,1,T]);

% recover Xbart
Xt=cell(T,1);
Xbart=cell(T,1);
for ii=1:T
Xt{ii,1}=X(ii,:);
Xbart{ii,1}=kron(speye(n),X(ii,:));
end


function [varargout]=constrainedDelaunayTetGen(V,C)

%% Create TetGen input structure

if ~isempty(C)
    inputStruct.Faces=C; %Add face constraints if not empty
    inputStruct.stringOpt='-pQY';
else
    inputStruct.stringOpt='-Q';
end
inputStruct.Nodes=V;
inputStruct.holePoints=[];
inputStruct.faceBoundaryMarker=ones(size(C,1),1); %Face boundary markers
inputStruct.regionPoints=[]; %region points

%% Run TetGen
[meshOutput]=runTetGen(inputStruct); %Run tetGen

TR = triangulation(meshOutput.elements,meshOutput.nodes);

switch nargout
    case 1
        varargout{1}=TR;
    case 2
        varargout{1}=TR;
        varargout{2}=meshOutput;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


classdef TopologyMonitoring < handle

    properties (Access = public)
        type
        designVariable
        cost
        constraint
        dualVariable
        historyPrinter
        convergenceVars
        monitorDocker
        postProcess
        incrementalScheme
        monitor
    end

    properties (Access = private)
        nIter
        hasFinished
        lineSearch
        lineSearchTrials
    end

    methods (Access = public)

        function obj = TopologyMonitoring(cParams)
            obj.init(cParams)
        end

        function compute(obj,cParams)
            switch obj.type
                case 'fmincon'
                    obj.plotFmincon(cParams);
                case 'NullSpace'
                    obj.plotNullSpace(cParams);
                case 'AlternatingPrimalDual'
                    obj.plotAugmentedLagrangian(cParams);
                case 'DualNestedInPrimal'
                    obj.plotBisection(cParams);
                case 'IPOPT'
                    obj.plotIPOPT(cParams);
                case 'MMA'
                    obj.plotMMA(cParams);
                otherwise
                    error('Optimizer not implemented')
            end
        end

        function create(obj,cParams)
            obj.createHistoryPrinter(cParams.historyPrinterSettings);
            obj.createConvergenceVariables(cParams);
            obj.createMonitorDocker(cParams.monitoringDockerSettings);
            obj.createPostProcess(cParams.postProcessSettings);
        end

    end

    methods (Access = private)

        function init(obj,cParams)
            obj.type     = cParams.type;
            obj.cost              = cParams.cost;
            obj.constraint        = cParams.constraint;
            obj.designVariable    = cParams.designVar;
            obj.dualVariable      = cParams.dualVariable;
            obj.incrementalScheme = cParams.incrementalScheme;
        end

        function plotFmincon(obj,cParams)
            obj.dualVariable.value = zeros(obj.constraint.nSF,1);
            obj.nIter              = cParams.nIter;
            obj.hasFinished        = cParams.hasFinished;
            foOpt                  = cParams.firstorderopt;
            normXsquare            = obj.designVariable.computeL2normIncrement();
            incX                   = sqrt(normXsquare);
            obj.designVariable.updateOld();
            switch cParams.algorithm
                case 'sqp'
                    stepL = cParams.stepsize;
                case 'interior-point'
                    stepL = cParams.trustregionradius;
                otherwise
            end
            obj.printOptimizerVariable();
            obj.convergenceVars.reset();
            obj.convergenceVars.append(incX);
            obj.convergenceVars.append(foOpt);
            obj.convergenceVars.append(stepL);
            obj.refreshMonitoring();
            obj.printHistory();
        end

        function plotNullSpace(obj,cParams)
            deltaCost              = obj.cost.value - cParams.oldCost;
            obj.nIter              = cParams.nIter;
            normXsquare            = obj.designVariable.computeL2normIncrement();
            obj.lineSearch         = cParams.tau;
            obj.lineSearchTrials   = cParams.lineSearchTrials;
            obj.hasFinished        = cParams.hasFinished;
            incX                   = sqrt(normXsquare);
            obj.designVariable.updateOld();
            obj.printOptimizerVariable();
            obj.convergenceVars.reset();
            obj.convergenceVars.append(deltaCost);
            obj.convergenceVars.append(incX);
            obj.convergenceVars.append(obj.lineSearch);
            obj.convergenceVars.append(obj.lineSearchTrials);
            obj.convergenceVars.append(cParams.meritNew);
            obj.refreshMonitoring();
            obj.printHistory();
        end

        function plotAugmentedLagrangian(obj,cParams)
            deltaCost              = obj.cost.value - cParams.oldCost;
            obj.nIter              = cParams.nIter;
            normXsquare            = obj.designVariable.computeL2normIncrement();
            obj.lineSearch         = cParams.tau;
            obj.lineSearchTrials   = cParams.lineSearchTrials;
            obj.hasFinished        = cParams.hasFinished;
            incX                   = sqrt(normXsquare);
            obj.designVariable.updateOld();
            obj.printOptimizerVariable();
            obj.convergenceVars.reset();
            obj.convergenceVars.append(deltaCost);
            obj.convergenceVars.append(incX);
            obj.convergenceVars.append(obj.lineSearch);
            obj.convergenceVars.append(obj.lineSearchTrials);
            obj.convergenceVars.append(cParams.meritNew);
            obj.refreshMonitoring();
            obj.printHistory();
        end

        function plotBisection(obj,cParams)
            deltaCost              = obj.cost.value - cParams.oldCost;
            obj.nIter              = cParams.nIter;
            normXsquare            = obj.designVariable.computeL2normIncrement();
            obj.lineSearch         = cParams.tau;
            obj.lineSearchTrials   = cParams.lineSearchTrials;
            obj.hasFinished        = cParams.hasFinished;
            incX                   = sqrt(normXsquare);
            obj.designVariable.updateOld();
            obj.printOptimizerVariable();
            obj.convergenceVars.reset();
            obj.convergenceVars.append(deltaCost);
            obj.convergenceVars.append(incX);
            obj.convergenceVars.append(obj.lineSearch);
            obj.convergenceVars.append(obj.lineSearchTrials);
            obj.refreshMonitoring();
            obj.printHistory();
        end

        function plotIPOPT(obj,cParams)
            obj.hasFinished = cParams.hasFinished;
            obj.nIter       = cParams.nIter;
            normXsquare     = obj.designVariable.computeL2normIncrement();
            obj.printOptimizerVariable();
            obj.convergenceVars.reset();
            obj.convergenceVars.append(cParams.inf_pr);
            obj.convergenceVars.append(cParams.inf_du);
            obj.convergenceVars.append(sqrt(normXsquare));
            obj.refreshMonitoring();
            obj.printHistory();
        end

        function plotMMA(obj,cParams)
            obj.hasFinished = cParams.hasFinished;
            obj.nIter       = cParams.nIter;
            obj.printOptimizerVariable();
            obj.convergenceVars.reset();
            obj.convergenceVars.append(cParams.KKTnorm);
            obj.convergenceVars.append(cParams.outitFrac);
            obj.refreshMonitoring();
            obj.printHistory();
        end

        function createHistoryPrinter(obj,cParams)
            cParams.optimizer  = obj;
            cParams.cost       = obj.cost;
            cParams.constraint = obj.constraint;
            obj.historyPrinter = OptimizationMetricsPrinterFactory.create(cParams);
        end

        function createConvergenceVariables(obj,cParams)
            s                   = cParams.optimizerNames;
            cVarD               = ConvergenceVarsDispatcher.dispatchNames(s);
            n                   = numel(cVarD);
            cVar                = ConvergenceVariables(n);
            obj.convergenceVars = cVar;
        end

        function createMonitorDocker(obj,s)
            s.designVariable  = obj.designVariable;
            s.dualVariable    = obj.dualVariable;
            s.cost            = obj.cost;
            s.constraint      = obj.constraint;
            s.convergenceVars = obj.convergenceVars;
            obj.monitorDocker = MonitoringDocker(s);
        end

        function createPostProcess(obj,cParams)
            if cParams.shallPrint
                d                  = obj.createPostProcessDataBase(cParams);
                d.printMode        = cParams.printMode;
                d.nDesignVariables = obj.designVariable.nVariables;
                obj.postProcess    = Postprocess('TopOptProblem',d);
            end
        end

        function d = createPostProcessDataBase(obj,cParams)
            d.mesh    = obj.designVariable.mesh;
            d.outName = cParams.femFileName;
            d.ptype   = cParams.ptype;
            ps = PostProcessDataBaseCreator(d);
            d  = ps.create();
            d.pdim    = cParams.pdim;
            d.ndim    = obj.designVariable.mesh.ndim;
            d.optimizer  = obj.type;
            d.cost       = obj.cost;
            d.constraint = obj.constraint;
            d.designVar  = obj.designVariable.type;
        end

        function printOptimizerVariable(obj)
            if ~isempty(obj.postProcess)
            d.fields     = obj.designVariable.getVariablesToPlot();
            d.cost       = obj.cost;
            d.constraint = obj.constraint;
%             obj.postProcess.print(obj.nIter,d);
            end
        end

        function printHistory(obj)
            iStep = obj.incrementalScheme.iStep;
            obj.historyPrinter.print(obj.nIter,iStep);
        end

        function printHistoryFinalValues(obj)
            obj.historyPrinter.printFinal();
        end

        function refreshMonitoring(obj)
            iStep = obj.incrementalScheme.iStep;
            nStep = obj.incrementalScheme.nSteps;
            obj.monitorDocker.refresh(obj.nIter,obj.hasFinished,iStep,nStep);
        end

    end

end


function [brs,delay] = ssr_brs_nfchoa(X,phi,xs,src,irs,conf)
%SSR_BRS_NFCHOA binaural room scanning (BRS) set for use with the SoundScape
%Renderer
%
%   Usage: [brs,delay] = ssr_brs_nfchoa(X,phi,xs,src,irs,conf)
%
%   Input parameters:
%       X       - listener position / m
%       phi     - azimuthal head orientation / rad
%       xs      - virtual source position [ys > Y0 => focused source] / m
%       src     - source type: 'pw' - plane wave
%                              'ps' - point source
%                              'fs' - focused source
%       irs     - impulse response data set for the second sources
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       brs     - conf.N x 2*nangles matrix containing all impulse responses (2
%                 channels) for every angles of the BRS set
%       delay    - delay added by driving function / s
%
%   SSR_BRS_NFCHOA(X,phi,xs,src,irs,conf) prepares a BRS set for a virtual
%   source at position xs for a virtual loudspeaker array driven by
%   nearfield compensated higher order Ambisonics (NFC-HOA) and the given
%   listener position. One way to use this BRS set is using the
%   SoundScapeRenderer (SSR), see http://spatialaudio.net/ssr/
%
%   See also: ir_generic, ir_nfchoa, driving_function_imp_nfchoa

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% Permission is hereby granted,  free of charge,  to any person  obtaining a *
% copy of this software and associated documentation files (the "Software"), *
% to deal in the Software without  restriction, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, subject to the following conditions:       *
%                                                                            *
% The above copyright notice and this permission notice shall be included in *
% all copies or substantial portions of the Software.                        *
%                                                                            *
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking of input  parameters ==================================
nargmin = 6;
nargmax = 6;
narginchk(nargmin,nargmax);
isargposition(X);
isargxs(xs);
isargscalar(phi);
isargstruct(conf);

%% ===== Computation =====================================================
% Secondary sources
x0 = secondary_source_positions(conf);
% Calculate driving function
[d,~,delay] = driving_function_imp_nfchoa(x0,xs,src,conf);
% Calculate brs set
brs = ssr_brs(X,phi,x0,d,irs,conf);


function params = makeRoughBars(params,id)
% makeRoughBars - make rough bar mapping stimulus
%
% params = makeRoughBars(stimparams,stimulus_id);
%
% Hack: copied almost straight from stimulus code. Little
% flexibility - good enough for now.
%
% 2007/09 SOD: wrote it.

if notDefined('params');     error('Need params'); end;
if notDefined('id');         id = 1;                   end;

fprintf(1,'[%s]:Processing:...',mfilename);drawnow;

fixMov = -5;
eyeMov = [20 10 20./180*pi]./2;
X = params.analysis.X;
Y = params.analysis.Y;

stimlimits = [14 14/600*800];

% make images
images=zeros(numel(X),21);
imii  = 1;

%--- fix: left up
X2 = X-fixMov;
Y2 = Y-fixMov;
% stim: right
X3 = X2 - eyeMov(1);
Y3 = Y2;
t  = atan2 (Y3, X3);
mask = t>=-pi/2+eyeMov(3) & t<=pi/2-eyeMov(3) &...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) = mask(:);
imii = imii+2;

% stim: bottom
X3 = X2;
Y3 = Y2 - eyeMov(2);
t  = atan2 (Y3, X3);
mask = t>=0+eyeMov(3) & t<=pi-eyeMov(3) &...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) = mask(:);
imii = imii+2;

%-- fix: right up
X2 = X+fixMov;
Y2 = Y-fixMov;
% stim: left
X3 = X2 + eyeMov(1);
Y3 = Y2;
t  = atan2 (Y3, X3);
mask = (t<=-pi/2-eyeMov(3) | t>=pi/2+eyeMov(3)) &...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii)= mask(:);
imii = imii+2;

% stim: bottom
X3 = X2;
Y3 = Y2 - eyeMov(2);
t  = atan2 (Y3, X3);
mask = t>=0+eyeMov(3) & t<=pi-eyeMov(3)&...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) = mask(:);
imii=imii+2;

%--- fix: center
X2 = X;
Y2 = Y;
% vertical
mask = abs(X2)<=eyeMov(1)./2 & ...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) =  mask(:);
imii=imii+2;
% horizontal
mask = abs(Y2)<=eyeMov(2)./2 &...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) =  mask(:);
imii=imii+2;

%--- fix: left down
X2 = X-fixMov;
Y2 = Y+fixMov;
% stim: right
X3 = X2 - eyeMov(1);
Y3 = Y2;
t  = atan2 (Y3, X3);
mask = t>=-pi/2+eyeMov(3) & t<=pi/2-eyeMov(3)&...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) =  mask(:);
imii=imii+2;
% stim: top
X3 = X2;
Y3 = Y2 + eyeMov(2);
t  = atan2 (Y3, X3);
mask = t>=-pi+eyeMov(3) & t<=0-eyeMov(3)&...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) = mask(:);
imii=imii+2;

%-- fix: right down
X2 = X+fixMov;
Y2 = Y+fixMov;
% stim: left
X3 = X2 + eyeMov(1);
Y3 = Y2;
t  = atan2 (Y3, X3);
mask = (t<=-pi/2-eyeMov(3) | t>=pi/2+eyeMov(3)) &...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) = mask(:);
imii=imii+2;

% stim: top
X3 = X2;
Y3 = Y2 + eyeMov(2);
t  = atan2 (Y3, X3);
mask = t>=-pi+eyeMov(3) & t<=0-eyeMov(3) &...
       abs(X)<=stimlimits(2) & ...
       abs(Y)<=stimlimits(1);
images(:,imii) = mask(:);
%imii=imii+2;

% make sequence
sequence = [5:8 9:12 13:16 21,21 17:20 9:12 3,4,1,2 21,21 15,16 13,14 11,12 9,10 7,8 5,6 21,21 19,20 17,18 11,12 9,10 1:4 21,21];
fixseq   = ceil(sequence./4);

sequence = reshape(sequence,2,numel(sequence)./2)';% pair up
sequence = sequence(:,1);
sequence = repmat(sequence',5,1);
sequence = sequence(:);

fixseq = reshape(fixseq,2,numel(fixseq)./2)';% pair up
fixseq = fixseq(:,1);
fixseq = repmat(fixseq,1,5);
fixseq = fixseq';
fixseq = fixseq(:);

% fill in fixseq with nearest values
while sum(fixseq==6),
    a = diff(fixseq==6);
    ii=find(a==1);
    fixseq(ii+1)= fixseq(ii);
    ii=find(a==-1);
    fixseq(ii) = fixseq(ii+1);
end

nuisance = zeros(numel(fixseq),10);
change = [0;diff(fixseq)~=0] -  [diff(fixseq)~=0;0];
counter =1;
for n=1:5,
    nuisance(:,counter) = fixseq==n & change ==1;
    if fixseq(1)==n, nuisance(1,counter) = 1; end;
    counter = counter + 1;
    nuisance(:,counter) = fixseq==n & change ==-1;
    if fixseq(end)==n, nuisance(end,counter) = 1; end;
    counter = counter + 1;
end

% all images
img = images(:,sequence);
preimg = img(:,1+end-params.stim(id).prescanDuration:end);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
doEyeCorrection = params.stim(id).stimStart(1)~=0;
doVelocityWeighting = false;
%velocityMax     = 200;
if doEyeCorrection,
    fprintf(1,'\n[%s]:Correcting for eye-movements.\n',mfilename);
else
    fprintf(1,'\n[%s]:NOT correcting for eye-movements.\n',mfilename);
end
% incorporate eyemovements
newimg = zeros(size(img));

% HACK: stored in non-used stimDir
scans = params.stim(id).stimStart;
scans = max(scans,1);
for scanid=scans,
    eyeTrackData = sprintf('%s/eyeMovements/scan%d_eyetrackdata.mat',pwd,scanid);
    e = eyetrack2deg_roughBars(eyeTrackData);
    if doVelocityWeighting,
        stepsize = mean(diff(unique(params.analysis.Y)));
        e.velocity = e.velocity/2.3548*stepsize/4;
    end
    if ~doEyeCorrection,
        % this provides a visual check of the motion correction, because
        % fixation is moved during the scan.
        e.eye = e.fixation;
        e.blinks = false(size(e.blinks));
    end

    % resample to stimulus sequence
    stimindex    = linspace(0,numel(e.blinks),numel(e.blinks)/(e.framerate.*params.stim(id).framePeriod)+1)+1;
    stimduration = e.framerate.*.2;
    X = reshape(params.analysis.X,[1 1]*sqrt(numel(params.analysis.X)));
    Y = reshape(params.analysis.Y,[1 1]*sqrt(numel(params.analysis.Y)));

    for n=1:size(img,2),
        if sum(img(:,n))~=0,
            tmp = zeros(size(X));
            zi  = tmp;
            tmp(:) = img(:,n);
            for n2=stimindex(n):stimindex(n)+stimduration-1,
                if ~e.blinks(n2)

                    % resample to 'corrected' position + give weight based
                    % on velocity.
                    newtmp = interp2(X,Y,tmp,X+e.eye(n2,1),Y+e.eye(n2,2),'*linear',0);
                    if doVelocityWeighting && doEyeCorrection,
                        % assume velocity = fwhm
                        if e.velocity(n2) > 1,
                            f = fspecial('gaussian',ceil(e.velocity(n2)*6),e.velocity(n2));
                            newtmp = imfilter(newtmp,f,'same');
                        end
                    end
                    zi = zi + newtmp;%.*w(n); % old way
                end
            end
            newimg(:,n) = newimg(:,n) + zi(:)./stimduration;
        end
    end
end
newimg = newimg./numel(scans);

% append prestimuli
params.stim(id).images = cat(2,preimg,newimg);
% store nuisance parameters convolved with the Hrf
params.stim(id).nuisance = [nuisance(1+end-params.stim(id).prescanDuration:end,:); nuisance];
params.stim(id).nuisance = filter(params.analysis.Hrf{id},1,params.stim(id).nuisance);
params.stim(id).nuisance = rmAverageTime(params.stim(id).nuisance, ...
                                         params.stim(id).nUniqueRep);
params.stim(id).nuisance = params.stim(id).nuisance(params.stim(id).prescanDuration+1:end,:);
params.stim(id).nuisance = params.stim(id).nuisance';
return;