clear;
clc;
close all;   

%% ==========================================================
% User Input
% File name (XXX.txt) and PixelsPerMM (XX) have to be specified
% ===========================================================

filename = 'XXX.txt';

% Calibration
pixelsPerMM = XX;   

% Conversion
pixel2mm = 1 / pixelsPerMM;

%% ==========================================================
% Read file
% ==========================================================

raw_matrix = readmatrix(filename, 'FileType', 'text', 'Delimiter', '\t');

x_raw = raw_matrix(:, 1);
y_raw = raw_matrix(:, 2); 

validIdx = ~isnan(x_raw) & ~isnan(y_raw);
x_clean = x_raw(validIdx);
y_clean = y_raw(validIdx);

x = x_clean * pixel2mm;
y = y_clean * pixel2mm;

%% ==========================================================
% Circle fit (LINEAR LEAST SQUARES)
% ===========================================================

A = [2*x, 2*y, ones(size(x))];
b = x.^2 + y.^2;

p = A \ b;

xc = p(1);
yc = p(2);
c  = p(3);

R = sqrt(c + xc^2 + yc^2);

%% ==========================================================
% Length of curve
% ===========================================================

dx = diff(x);
dy = diff(y);

arcLength = sum(sqrt(dx.^2 + dy.^2));

%% ==========================================================
% Curvature
% ===========================================================

kappa = 1 / R;

%% ==========================================================
% RMSE of the circle fit
% ===========================================================

distances = sqrt((x - xc).^2 + (y - yc).^2);

residuals = distances - R;

rmse = sqrt(mean(residuals.^2));

%% ==========================================================
% Results
% ===========================================================

fprintf('\n');
fprintf('====================================================\n');
fprintf('                Circle fit analysis\n');
fprintf('====================================================\n');
fprintf('File                   : %s\n', filename);
fprintf('Number of points       : %d\n', length(x));
fprintf('Pixel-to-mm factor     : %.6f mm/Pixel\n', pixel2mm);
fprintf('\n');
fprintf('Center of circle Xc    : %.4f mm\n', xc);
fprintf('Center of circle Yc    : %.4f mm\n', yc);
fprintf('Radius R               : %.4f mm\n', R);
fprintf('Length of curve        : %.4f mm\n', arcLength);
fprintf('Curvature kappa        : %.6e 1/mm\n', kappa);
fprintf('RMSE                   : %.6f mm\n', rmse);
fprintf('RMSE / R               : %.6e\n', rmse/R);
fprintf('====================================================\n');

%% ==========================================================
% Visualization of circle fit
% ===========================================================

theta = linspace(0,2*pi,2000);

xCircle = xc + R*cos(theta);
yCircle = yc + R*sin(theta);

figure('Color','w');

plot(x, y, 'b.', 'MarkerSize', 10);
hold on;

plot(xCircle, yCircle, 'r-', 'LineWidth', 1.5);

plot(xc, yc, 'ko', ...
    'MarkerFaceColor', 'k', ...
    'MarkerSize', 6);

axis equal;
grid on;
box on;

xlabel('x [mm]');
ylabel('y [mm]');

title(sprintf(['Radius = %.2f mm | ', ...
               '\\kappa = %.4e 1/mm | ', ...
               'RMSE = %.4f mm'], ...
               R, kappa, rmse));

legend('Measurement points', ...
       'Circle fit', ...
       'Circle center', ...
       'Location', 'best');

%% ==========================================================
% Residual plot
% ===========================================================

figure('Color','w');

plot(residuals, 'LineWidth', 1.2);

grid on;
box on;

xlabel('Point index');
ylabel('Deviation from the circle [mm]');

title(sprintf('Residuals of the circle fit (RMSE = %.4f mm)', rmse));

%% ==========================================================
% Quality assessment
% ===========================================================

relativeRMSE = rmse / R; 

fprintf('\n');