opt

%В конец вашего кода;
axis equal
hold on

%Попытка оптимизации без сопряжения производных
%Как я понял cpoints - это средняя линия, points2 - верхняя сторона
k1 = 1.35;
%Лучше крайние точки дуг включить также в массивы с верхней и нижней стороной
points = [pointsl(end, :); points2; pointsr(end, :)];
err = @(h) erfun(h, points, k1, cpoints, 1e-6);
%Когда будет условие сопряжения, через него будут узнаваться крайние высоты
%В таком случае будет так:

%err = @(h) erfun([h1,h,h2], points, k1, cpoints, 1e-6);
%h1, h2 - крайние высоты


%Быстрый тест
h = [0.2 0.2 0.2 0.2];
err(h)

%Базовая оптимизация c ограничением по h(Функция err(hl))
%Без ограничений получается ерунда
[h_res, funvalue] = fmincon(err, h, [], [], [], [], [0 0 0 0], [1 1 1 1])
%Проверка
cp_res = cp_up(cpoints, k1, h_res, pointsl(end, :), pointsr(end, :))
t = 0:0.01:1;
res_points = bezierPoints(cp_res,t);
plot(res_points(:,1),res_points(:,2))
hold off



%%%Для оптимизации:

%Уравнение относительно оптимального значения параметра
function [value] = eq_for_t(t,cpoints,dataPoint)
    dif = bezierPoints(cpoints,t) - dataPoint;
    value = dot(dif,bezierPrime(cpoints,t));
end

%Метод половинного деления
function [x] = bisect(fun,tol)
    left = 0;
    right = 1;
    while (right-left)>tol
        mid = (right+left)/2;
        if fun(mid) == 0
            x = mid;
            return
        elseif sign(fun(mid)) == sign(fun(left))
            left=mid;
        else
            right=mid;
        end
    end
    x = (right+left)/2;
end


%Рассчет меры совпадения по контрольным точкам кривой Безье
%points - точки, к которым приближаем
function [error] = error(cpoints, points, tol)

    %Поиск оптимального t для множества точек
    optimal_t = zeros(length(points));
    for i=1:length(points)
        point = points(i,:);
        funn = @(x) (eq_for_t(x,cpoints,point));
        optimal_t(i) = bisect(funn,tol);
    end

    %Рассчет ошибки как суммы расстояний, деленной на кол-во точек
    curvePoints = bezierPoints(cpoints,optimal_t);
    distVectors = (curvePoints - points);
    error = 0;
    for i=1:length(distVectors)
        error = error + norm(distVectors(i, :));
    end
    error = error / length(distVectors);
end

%Получение координат контрольных точек из высот и коэф.сгущения
function [cp_up] = cp_up(cp_mid,k1,h1,st_point,end_point)
    %Из вашего кода
    n1 = length(h1) + 2;
    s1 = 1 / (k1^(n1 - 2));
    t1 = zeros(n1);
    cp_up = zeros([n1, 2]);
    for i = 2:n1
        t1(i) = k1^(i - 2) * s1;
    end
    midPoints = bezierPoints(cp_mid,t1);
    cp_up(1, :) = st_point;
    cp_up(n1, :) = end_point;
    for i = 2 : (n1 - 1)
        f = bezierPrime(cp_mid,t1(i));
        cp_up(i, 2) = midPoints(i, 2) + h1(i-1) / sqrt(1 + (f(1, 2))^2);
        cp_up(i, 1) = midPoints(i, 1) + sqrt(h1(i-1)^2 - (cp_up(i, 2) - midPoints(i, 2))^2);
    end
end

%Функция, непосредственно загружаемая в оптимизатор
function [value] = erfun(h, points, coef, cp_mid, bisect_tol)
    %Пока только для верхних, но это легко расширяется
    cpCurve = cp_up(cp_mid, coef, h, points(1, :), points(end, :));
    value = error(cpCurve, points, bisect_tol);
end