%clear all, close all, clc
%parpool
MaxIter=2000;
MaxFunctionEvaluations=50000;
TolFro=1e-12;
TolXs=1e-12;

%optionsf = optimoptions('fmincon','TolFun',TolFro,'TolX',TolXs,...
optionsf = optimoptions('fminimax','TolFun',TolFro,'TolX',TolXs,...
'FiniteDifferenceStepSize',eps^(1/3),...
'ConstraintTolerance',1e-3,...
'MaxIter',MaxIter,...
'MaxFunctionEvaluations',MaxFunctionEvaluations,...
'UseParallel',false);%FunctionTolerance
%,'Algorithm','sqp'%'FiniteDifferenceType','central',...
%%
xopsw=[   0.046334284562866
  -5.355010042230490
   0.113524936450297
  -2.035906118674400
   4.954170143877920
   1.550635410871368
   5.165420748579254
   2.435748668787137
   2.674809901488776
   0.149457073807350
   0.846646376468854
   0.743743990645261
   0.054642291958716
   0.355987019879191
  -0.237653168642405
   0.125572488975305
   0.566559485228246
   1.113299684743702
   1.995390842602884
  -0.779869923535340
   0.535150016261177
   0.549135727176047
   0.248226304619958];
computevcceqmm(xopsw)
%%
% xopsw=[   0.150839428316863
%   -1.532694026145179
%    0.400728615833992
%    4.147029104746636
%    0.873898355392351
%    1.807660565697757
%    2.567481935603491
%    0.001382637534955
%    0.000092178792944
%   -0.138894595406779
%    2.660952358612419
%   -0.003860486696993
%   -0.011024194533283
%    0.532493342838555
%   -0.000029243969851];
xopsw=[   0.483402045959110
  -1.134494733854711
  -0.268527211814195
  -3.727902170264331
   6.782970042548285
   6.134937292875161
   6.569144892074585
   2.964828516208100
   8.219661473273501
   0.202109131115497
   0.299803329094701
   0.445535418216211
   0.422019223863767
   0.708149011360256
  -0.291796363655602
   0.231333571978532
   0.463039967200092
   1.762389722228661
   2.837783975548761
  -0.633938118750967
   0.296011588214347
   0.396863765140706
   0.374361969361109];
computevcceq(xopsw)
%phasphi=x(1:2);
% Aznom=x(3);
% tau0=x(4);
% kdtau=x(5);
% kpLe=x(6);
% kpAz=x(7);
% kdLe=x(8);
% kdAz=x(9);
% I_bod=diag(x(11:13));
% Lrv=[x(14);x(15);x(10)];
%%
% xopsw2 =[   0.090892807730532
%   -1.623039036196829
%    0.302003460372148
%  -18.885708162836064
%    4.925454738421157
%    2.662762743930492
%    2.244247772615900
%   11.437523083253529
%    2.709370610696761
%    0.311889118849127
%    0.2
%    0.15
%    0.1
%    0.028402022089646
%    0.063886272380007
%    0.094136731554881
%    2.576460204590125
%   -0.131183154293990
%   -0.010343689590803
%    0.221075375589269
%    0.009087579530106]
% computevcceq(xopsw2)
% (phasphi=x(1:2);
% Aznom=x(3);
% tau0=x(4);
% kdtau=x(5);
% kpLe=1900*x(6);
% kpAz=190*x(7);
% kdLe=x(8);
% kdAz=x(9);
% Lr=x(10);
% l0=x(11);
% m=x(12);)I_bod=1*diag([0.2,0.15,0.1]);
%%
%xopsw=BestSol.Position;
%xopsw2 = fmincon(@(x) FunCostFull(x,1e+4), xopsw,[],[]);%optionsf
%xopsw2 = fmincon(@(x) FunCostFloq([x;xopsw(3:end)],1e+10), xopsw(1:2),[],[],[],[],[],[],@(x) HybridcostMod([x;xopsw(3:end)],1e+04),optionsf);%
%xopsw2 = fmincon(@(x) FunCostFloq(x,wrV), xopsw,[],[],[],[],[],[],@(x) conHybridcostMod(x,1e+04),optionsf);%
%xopsw2 = fmincon(@(x) FunCostFull([x;xopsw(3:end)],1e+10), xopsw(1:2),[],[],[],[],[],[],[],optionsf);%
[xopsw2,f,eflag,outpt]=runobjcons(xopsw+.00001*(2*rand(size(xopsw))-1),optionsf);%xopsw+.1*(2*rand(size(xopsw))-1)

function [x,f,eflag,outpt] = runobjcons(x0,opts)
gv=[];g=[];
tpHM0=[];
l0=[];
m=[];

InitSetParam;
if nargin == 1
    opts = [];
end
xLast = [];
myf = [];
myc = [];
myceq = [];
fun = @objfun;
cfun = @constr;
lb=-ones(size(x0))*Inf;
ub=ones(size(x0))*Inf;
ub(4)=0;
lb(5:10)=0;
lb(11:13)=0.001;
ub(11:13)=0.25*m;
lb(14)=0;
ub(14)=0.36;
%lb(16)=-1.55;
%ub(16)=1.55;
%lb(18)=eps;ROT AROUND Z FUCK THISSSSSSSSSSS
%[x,f,eflag,outpt] = fmincon(fun,x0,[],[],[],[],lb,ub,cfun,opts);
[x,f,eflag,outpt] = fminimax(fun,x0,[],[],[],[],lb,ub,cfun,opts);
f
    function y = objfun(x)
        if ~isequal(x,xLast)
            [myf,myc,myceq] = computevcceqmm(x);
            xLast = x;
        end
        y = myf;
    end
    function [c,ceq] = constr(x)
        if ~isequal(x,xLast)
            [myf,myc,myceq] = computevcceqmm(x);
            xLast = x;
        end
        c = myc;
        ceq = myceq;
    end
end
% %%
% xopsw=[   0.082668007296833
%   -2.020747563010401
%    0.325663090665842
%  -18.867370473319305
%    5.334722887489529
%    2.408310953747193
%    2.207564389343120
%   11.598717342283583
%    2.164074466510142
%    0.307035599767812
%    0.209577832429365
%    0.184409636646938
%    0.129077525070308
%    0.046080540035055
%    0.074824422625444
%    0.086230320369700
%    2.328134446322812
%   -0.209390226539513
%   -0.011263183624858
%    0.332373375817849
%    0.008680268399997];
% computevcceq(xopsw)
% % phasphi=x(1:2);
% % Aznom=x(3);
% % tau0=100*x(4);
% % kdtau=100*x(5);
% % kpLe=1900*x(6);
% % kpAz=190*x(7);
% % kdLe=100*x(8);
% % kdAz=100*x(9);
% % Lr=x(10);
% % I_bod=diag(x(11:13));
%% 0.538WTF
%   -0.090163422759212
%   -3.176882548592152
%    0.893056510195970
%   -2.339154835499486
%    0.312490549245554
%    4.179564786681737
%    3.239878667897109
%    1.198868408095725
%    0.816481561128378
%    0.134993853996689
%    0.946526334522628
%    2.535092337290350
%    0.630622840604687
%    0.353619863882432
%    0.653404022455421
%   -1.383049101839949
%    2.176688966430349
%   -1.094278212285816
%   -0.816655350180551
%    3.383573164648610
%    0.024010038484091
%%GG
% -0.107616069773681
%   -4.714449925520523
%    1.290245459192239
%   -8.127055061889195
%    0.783123924138724
%    8.689534418757582
%    7.530237117998920
%    5.704436588237513
%    7.500576524247700
%    0.014763352909699
%    4.387907171935271
%    3.118828517503618
%    2.115808445247445
%    0
%    0.401483331603652
%    0.668702843844361
%   -2.260035432040231
%    4.359664321844581
%   -1.588767716890935
%   -0.459987022411215
%    6.537264959825839
%    -0.077608784503321
%% "BEST" LOL
% xopsw=[   0.090892807730532
%   -1.623039036196829
%    0.302003460372148
%  -18.885708162836064
%    4.925454738421157
%    2.662762743930492
%    2.244247772615900
%   11.437523083253529
%    2.709370610696761
%    0.311889118849127
%    0.2
%    0.15
%    0.1
%    0
%    0.028402022089646
%    0.063886272380007
%    0.094136731554881
%    2.576460204590125
%   -0.131183154293990
%   -0.010343689590803
%    0.221075375589269
%    0.009087579530106];
% computevcceq(xopsw)
%%0.781
%   -0.125286517679160
%   -4.000539553812143
%    0.902119651475858
%  -19.932943332953560
%    5.043540557685549
%    2.887396002285181
%    4.477995668891322
%   11.726898238001889
%    4.698186048289099
%    0.036084717737342
%    2.047637636544711
%    0.431418724163609
%    0.256211588890985
%   -0.066399316848874
%    0.167770655395476
%    0.383367897928499
%    0.368768605445864
%    1.541143780637317
%   -0.651597476087390
%   -0.004370361622998
%    0.841899921039575
%    0.033857644429185
%%0.833
%   -0.064150635005847
%  -19.108303219826684
%    0.760630011654716
%  -30.554068556099992
%   26.799598509840255
%   28.099271617787679
%   31.596091005081032
%   15.344000358554887
%   38.708872661174524
%    0.265898228808290
%    0.089446150408558
%    0.631584033525886
%    0.754487338168032
%   -0.065449621699436
%    0.033580696067617
%    0.202196219565821
%    0.438510290781963
%    0.472359260843229
%   -0.452394718795771
%    0.000815164593892
%    0.449041970782280
%    0.001112736771577
%%0.844
%   -0.076066207092251
%  -19.108361357167890
%    0.741379229400108
%   -3.073126068785609
%    2.676538222167804
%    2.809020216309439
%    3.159198422759733
%    1.529314860359108
%    3.870868176456027
%    0.264389348148807
%    0.092137174523102
%    0.641442958127601
%    0.751155884849456
%   -0.065582321915142
%    0.003616603416759
%    0.035412492747920
%    0.212554568629391
%    0.440243521385467
%    0.473367000165256
%   -0.482576260902485
%    0.000857412274079
%    0.451402939869978
%    0.001097058871305
%%like the old times baby, like the old times
% xopsw=1000*[   0.000082362529106
%   -0.003548726958408
%    0.000026891124224
%   -0.020959441020198
%    0.002781200658455
%    0.1000862204198936
%    0.040361883555655
%    0.100512369957437
%    0.005020541536409
%    0.000251059876509
%    0.000580403854587
%    0.000572476045246
%    0.000226063660699
%    0.000298634989408
%   -0.000173344749201
%    0.000134899230591
%    0.000557267127089
%    0.000890273109598
%    0.002606036180881
%   -0.000821067634111
%    0.000385671043458
%    0.000257000071561
%    0.000271850844502];
% computevcceq(xopsw)
%%0.6
%    1.0e+02 *
% 
%   -0.000112341972486
%   -0.037288481838217
%    0.000663785539461
%   -0.211675323476306
%    0.031634132619118
%    1.000937260203923
%    0.404345071941398
%    1.005224765430517
%    0.051517990924977
%    0.002822471500829
%    0.006590814451952
%    0.007756698478929
%    0.002875203028683
%    0.003587281817112
%   -0.001538434005171
%    0.001653854002405
%    0.007253572599260
%    0.010537064974476
%    0.023396705375641
%   -0.008150829641210
%    0.004590581826340
%    0.005297765985493
%    0.002895538251976
%%0.488
%    0.249430638765950
%   -2.516981308669759
%   -0.030302777734227
%  -21.441926112175160
%    3.455294925557660
%   10.071138826756037
%   40.629346243246339
%    9.867714618129501
%    5.821867126309155
%    0.112001506397656
%    0.663330270632282
%    0.809268619641016
%    0.197472065854721
%    0.515339930693045
%   -0.293747286784779
%    0.156129444622235
%    0.537902850062866
%    1.332686007644574
%    2.432317023156028
%   -0.755765773293853
%    0.581845667737733
%    0.672390926154516
%    0.372570112591817
%less
%    0.386582272841627
%   -1.883981913396615
%   -0.111934159983090
%   -3.068659879991947
%    4.436384593310159
%    4.705524487532916
%    5.769917387554830
%   10.454222980201006
%    6.573504534710710
%    0.197399576787729
%    0.477619157635542
%    0.720291539990814
%    0.268840883386430
%    0.602503085523552
%   -0.342447788279887
%    0.198202980536666
%    0.509387101937465
%    1.729042002525669
%    2.893397490102653
%   -0.883282766282111
%    0.570348289612819
%    0.742101747296800
%    0.531796978481552
%0.387
%    0.422999402801213
%   -2.016973271716771
%   -0.123595499694105
%   -3.291419569455309
%    4.455610640689931
%    5.012719083029448
%    6.137583747447335
%    1.0591905136806201
%    6.735500338652742
%    0.200444152023714
%    0.484242033961424
%    0.762841874898648
%    0.296507443893158
%    0.586708253928079
%   -0.365696340109015
%    0.195685577600936
%    0.496187670713379
%    1.796000960281140
%    3.003241556827799
%   -0.992376998340741
%    0.616220669133600
%    0.735318456588204
%    0.581919754424741
%0.374
%    0.530191918614372
%   -2.087989748008173
%   -0.149721718028148
%   -3.486808955043617
%    4.464783191960289
%    5.052530567214323
%    6.294208616701076
%    1.163969241161774
%    6.806020241454490
%    0.195944491551456
%    0.484595248127903
%    0.679626766373393
%    0.332202385836268
%    0.555923172418293
%   -0.390627670156373
%    0.211358498252597
%    0.431196867451312
%    1.898114205924760
%    3.040375607778115
%   -1.088624977778504
%    0.597480329436548
%    0.579182845355803
%    0.591132553831013
%0.345

%    0.264135217523728
%   -1.770417599859085
%   -0.073891167734869
%   -4.100511306446566
%    4.773726930247615
%    5.479083761145508
%    6.336181459568794
%    2.160579406236320
%    7.468214199915487
%    0.282393764429209
%    0.350826483160081
%    0.796374961077392
%    0.381411829782008
%    0.610149306681695
%   -0.312173572580003
%    0.171551209525788
%    0.646786608925754
%    1.824371671762753
%    3.598578798953662
%   -0.866509020651636
%    0.539063827957576
%    0.517767634067800
%    0.711152336395593
% 0.332
%    0.298018213832475
%   -1.447103672757478
%   -0.130039830214402
%   -4.160362665067960
%    5.622324329630309
%    6.087699538989451
%    6.861596504106124
%    2.501506653257143
%    7.695124848937406
%    0.331083534827247
%    0.377275665513961
%    0.729999936807322
%    0.281549120662263
%    0.595855098796194
%   -0.281065548365161
%    0.190398398611455
%    0.589940542549217
%    1.805223657246793
%    3.238721219599646
%   -0.909424655528873
%    0.456150211552132
%    0.686149763194224
%    0.673462597054006
%0.2
% 0.371838465427869
%   -1.164887927817917
%   -0.173775273921454
%   -4.024672985579360
%    5.670115198475112
%    6.199044863404709
%    6.916985247432411
%    2.516178085462577
%    8.115458240006941
%    0.282909629532182
%    0.352364736365763
%    0.560648549839667
%    0.327984066281609
%    0.687685043841390
%   -0.321903931378708
%    0.226917858727854
%    0.591877336266853
%    1.966792016512605
%    3.269526018282940
%   -0.787498246690045
%    0.476893895799075
%    0.613299828246175
%    0.558907736052935
%
% 0.somethign
% 0.392450011875433
%   -0.987179498122201
%   -0.195884413235364
%   -3.779615900507469
%    6.101639110498043
%    6.279673795035896
%    7.084598591378736
%    2.544062422829534
%    8.131678910856996
%    0.269384339056468
%    0.291921963555935
%    0.388037867386894
%    0.359791109365532
%    0.701101277305731
%   -0.320014815467038
%    0.253975174856070
%    0.588005163149669
%    1.982968298870573
%    3.038945985319257
%   -0.675297457756138
%    0.444912686118144
%    0.505202892871430
%    0.337991314542233

%0.206557022505565
%    0.358616174348894
%   -1.085853630592585
%   -0.265151861865421
%   -3.834739747490389
%    6.633289243974410
%    6.308735423824133
%    7.095558023539156
%    2.636782808276787
%    8.314209723993091
%    0.253026773386872
%    0.285586364226203
%    0.449202982652826
%    0.289388213927518
%    0.703514640408414
%   -0.247642279870319
%    0.203847796681629
%    0.520630399129549
%    1.756830851660002
%    2.941960666242937
%   -0.616810723875492
%    0.288003870659886
%    0.449217042789715
%    0.381697567734415
%%0.149
% 0.370152870565441
%   -1.134594672276226
%   -0.245274819399123
%   -3.848906690612655
%    6.776041036540288
%    6.198440772772403
%    7.040609557456170
%    2.702002077687264
%    8.332087744331288
%    0.253377046576483
%    0.258461073013311
%    0.433123210790387
%    0.365967683817842
%    0.699099424408978
%   -0.255287260437172
%    0.213046552281215
%    0.525251565771473
%    1.766712167403006
%    2.939870274802229
%   -0.599941524174483
%    0.299862093873401
%    0.420545142801454
%    0.365199832108714
%0.136
% 0.375808606463110
%   -1.160478177983596
%   -0.244932134502591
%   -3.793094702083049
%    6.791328832935200
%    6.204048915649577
%    6.886833311313152
%    2.938493737316198
%    8.377244526708571
%    0.247023278981305
%    0.255915939460361
%    0.457337891400258
%    0.409044463781844
%    0.702158726939541
%   -0.253170895103064
%    0.210353553632044
%    0.518051519287560
%    1.738969902359387
%    2.903504531771541
%   -0.587003211422219
%    0.293801419045781
%    0.424816720094153
%    0.372562473316811
% 0.1278
%    0.483397055903743
%   -1.134343127495625
%   -0.268525625433208
%   -3.727933935240975
%    6.782958717916259
%    6.134935215149810
%    6.569154286257372
%    2.964785485359294
%    8.219615837959076
%    0.202114759884841
%    0.299788086140990
%    0.445537086027522
%    0.422025648401381
%    0.708143553680262
%   -0.291794414451833
%    0.231333517451823
%    0.463043011667354
%    1.762390097409124
%    2.837809691687584
%   -0.633946236438543
%    0.296015707700190
%    0.396861682654534
%    0.374369526786691
%0.117
%    0.483402045959110
%   -1.134494733854711
%   -0.268527211814195
%   -3.727902170264331
%    6.782970042548285
%    6.134937292875161
%    6.569144892074585
%    2.964828516208100
%    8.219661473273501
%    0.202109131115497
%    0.299803329094701
%    0.445535418216211
%    0.422019223863767
%    0.708149011360256
%   -0.291796363655602
%    0.231333571978532
%    0.463039967200092
%    1.762389722228661
%    2.837783975548761
%   -0.633938118750967
%    0.296011588214347
%    0.396863765140706
%    0.374361969361109