%% Calibration function for 1 camera
function cameraParams = cameraIntrinsicsFromCheckerBoard(imageFileNames)

    % Detect calibration pattern in images
    detector = vision.calibration.monocular.CheckerboardDetector();
    minCornerMetric = 0.150000;
    [imagePoints, imagesUsed] = detectPatternPoints(detector, imageFileNames, 'MinCornerMetric', minCornerMetric);
    imageFileNames = imageFileNames(imagesUsed);

    % Read the first image to obtain image size
    originalImage = imread(imageFileNames{1});
    [mrows, ncols, ~] = size(originalImage);

    % Generate world coordinates for the planar pattern keypoints
    squareSize = 1.000000;  % in millimeters
    worldPoints = generateWorldPoints(detector, 'SquareSize', squareSize);

    % Calibrate the camera
    [cameraParams, ~, ~] = estimateCameraParameters(imagePoints, worldPoints, ...
    'EstimateSkew', false, 'EstimateTangentialDistortion', true, ...
    'NumRadialDistortionCoefficients', 3, 'WorldUnits', 'millimeters', ...
    'InitialIntrinsicMatrix', [], 'InitialRadialDistortion', [], ...
    'ImageSize', [mrows, ncols]);

end

%% Stereo calibration function
function stereoParams = stereoExtrinsicsFromCheckerBoards(imageFileNames1, camera1Intrinsics, imageFileNames2, camera2Intrinsics)

    % Detect calibration pattern in images
    detector = vision.calibration.stereo.CheckerboardDetector();
    minCornerMetric = 0.150000;
    [imagePoints, ~] = detectPatternPoints(detector, imageFileNames1, imageFileNames2, 'MinCornerMetric', minCornerMetric);

    % Generate world coordinates for the planar pattern keypoints
    squareSize = 1.000000;  % in millimeters
    worldPoints = generateWorldPoints(detector, 'SquareSize', squareSize);

    % Calibrate the camera
    [stereoParams, ~, ~] = estimateStereoBaseline(imagePoints, worldPoints, ...
        camera1Intrinsics, camera2Intrinsics, ...
        'WorldUnits', 'millimeters');
end


%% Center camera calibration
imageFileNames = {'checkerboards/center\center\0.png',...
                  'checkerboards/center\center\1.png',...
                  'checkerboards/center\center\2.png',...
                  'checkerboards/center\center\3.png',...
                  'checkerboards/center\center\4.png',...
                  'checkerboards/center\center\5.png',...
                  'checkerboards/center\center\6.png',...
                  'checkerboards/center\center\7.png',...
                  'checkerboards/center\center\8.png',...
                  'checkerboards/center\center\9.png',...
                  'checkerboards/center\Center\10.png',...
                  'checkerboards/center\Center\11.png',...
                  'checkerboards/center\Center\12.png',...
                  'checkerboards/center\Center\13.png',...
                  'checkerboards/center\Center\14.png',...
                  'checkerboards/center\Center\15.png',...
                  'checkerboards/center\Center\16.png',...
                  };

cameraParams_center = cameraIntrinsicsFromCheckerBoard(imageFileNames);

%% Left camera calibration

imageFileNames = {'checkerboards/left\left\0.png',...
                  'checkerboards/left\left\1.png',...
                  'checkerboards/left\left\2.png',...
                  'checkerboards/left\left\3.png',...
                  'checkerboards/left\left\4.png',...
                  'checkerboards/left\left\5.png',...
                  'checkerboards/left\left\6.png',...
                  'checkerboards/left\left\7.png',...
                  'checkerboards/left\left\8.png',...
                  'checkerboards/left\left\9.png',...
                  'checkerboards/left\left\10.png',...
                  'checkerboards/left\left\11.png',...
                  'checkerboards/left\left\12.png',...
                  'checkerboards/left\left\13.png',...
                  'checkerboards/left\left\14.png',...
                };

cameraParams_left = cameraIntrinsicsFromCheckerBoard(imageFileNames);

%% Right camera calibration

imageFileNames = {'checkerboards/right\right\0.png',...
                  'checkerboards/right\right\1.png',...
                  'checkerboards/right\right\2.png',...
                  'checkerboards/right\right\3.png',...
                  'checkerboards/right\right\4.png',...
                  'checkerboards/right\right\5.png',...
                  'checkerboards/right\right\6.png',...
                  'checkerboards/right\right\7.png',...
                  'checkerboards/right\right\8.png',...
                  'checkerboards/right\right\9.png',...
                  'checkerboards/right\right\10.png',...
                  'checkerboards/right\right\11.png',...
                  'checkerboards/right\right\12.png',...
                  'checkerboards/right\right\13.png',...
                  'checkerboards/right\right\14.png',...
                };

cameraParams_right = cameraIntrinsicsFromCheckerBoard(imageFileNames);

%% Center + Left stereo calibration
imageFileNames1 = {'checkerboards/center_left\center\0.png',...
                   'checkerboards/center_left\center\1.png',...
                   'checkerboards/center_left\center\10.png',...
                   'checkerboards/center_left\center\11.png',...
                   'checkerboards/center_left\center\12.png',...
                   'checkerboards/center_left\center\2.png',...
                   'checkerboards/center_left\center\3.png',...
                   'checkerboards/center_left\center\4.png',...
                   'checkerboards/center_left\center\5.png',...
                   'checkerboards/center_left\center\6.png',...
                   'checkerboards/center_left\center\7.png',...
                   'checkerboards/center_left\center\8.png',...
                   'checkerboards/center_left\center\9.png',...
                   };

imageFileNames2 = {'checkerboards/center_left\left\0.png',...
                   'checkerboards/center_left\left\1.png',...
                   'checkerboards/center_left\left\10.png',...
                   'checkerboards/center_left\left\11.png',...
                   'checkerboards/center_left\left\12.png',...
                   'checkerboards/center_left\left\2.png',...
                   'checkerboards/center_left\left\3.png',...
                   'checkerboards/center_left\left\4.png',...
                   'checkerboards/center_left\left\5.png',...
                   'checkerboards/center_left\left\6.png',...
                   'checkerboards/center_left\left\7.png',...
                   'checkerboards/center_left\left\8.png',...
                   'checkerboards/center_left\left\9.png',...
                   };

stereoParams_center_left = stereoExtrinsicsFromCheckerBoards(imageFileNames1, cameraParams_center, imageFileNames2, cameraParams_left);

%% Center + Right stereo calibration
imageFileNames1 = {'checkerboards/center_right\center\0.png',...
                   'checkerboards/center_right\center\1.png',...
                   'checkerboards/center_right\center\2.png',...
                   'checkerboards/center_right\center\3.png',...
                   'checkerboards/center_right\center\4.png',...
                   'checkerboards/center_right\center\5.png',...
                   'checkerboards/center_right\center\6.png',...
                   'checkerboards/center_right\center\7.png',...
                   };

imageFileNames2 = {'checkerboards/center_right\right\0.png',...
                   'checkerboards/center_right\right\1.png',...
                   'checkerboards/center_right\right\2.png',...
                   'checkerboards/center_right\right\3.png',...
                   'checkerboards/center_right\right\4.png',...
                   'checkerboards/center_right\right\5.png',...
                   'checkerboards/center_right\right\6.png',...
                   'checkerboards/center_right\right\7.png',...
                   };

stereoParams_center_right = stereoExtrinsicsFromCheckerBoards(imageFileNames1, cameraParams_center, imageFileNames2, cameraParams_right);


%% Save the parameters to a JSON

% Intrinsics for each camera
calib = struct();
calib.cameras = {'left', 'center', 'right'};

calib.intrinsics.center = pack_cam_params(cameraParams_center);
calib.intrinsics.left   = pack_cam_params(cameraParams_left);
calib.intrinsics.right  = pack_cam_params(cameraParams_right);

% Extrinsics (stereo)
calib.extrinsics.left.translation = stereoParams_center_left.TranslationOfCamera2(:);
calib.extrinsics.left.rotation = stereoParams_center_left.RotationOfCamera2'; % OpenCV Format is transposed

calib.extrinsics.right.translation = stereoParams_center_right.TranslationOfCamera2(:);
calib.extrinsics.right.rotation = stereoParams_center_right.RotationOfCamera2'; % OpenCV Format is transposed


% Save to JSON for Python / NeRF code
jsonStr = jsonencode(calib, 'PrettyPrint', true);
fid = fopen('calibration_data_matlab.json', 'w');
fprintf(fid, '%s\n', jsonStr);
fclose(fid);



%% Pack camera parameters into a simpler struct
function cam = pack_cam_params(cp)
    % Focal length and principal point (in pixels)
    cam.fx = cp.FocalLength(1);
    cam.fy = cp.FocalLength(2);

    % Shift principal point by -1 to map from MATLAB indexing (1-based)
    % to the more common OpenCV pixel coordinate convention.
    cam.cx = cp.PrincipalPoint(1) - 1;
    cam.cy = cp.PrincipalPoint(2) - 1;

    % K matrix
    cam.K = [cam.fx, 0,  cam.cx;
             0,  cam.fy, cam.cy;
             0,  0,  1];

    % Distortion parameters from MATLAB
    k = cp.RadialDistortion(:).';       % [k1 k2 (k3)]
    p = cp.TangentialDistortion(:).';   % [p1 p2]

    % Default zeros if not present
    cam.k1 = 0; cam.k2 = 0; cam.k3 = 0;
    if numel(k) >= 1, cam.k1 = k(1); end
    if numel(k) >= 2, cam.k2 = k(2); end
    if numel(k) >= 3, cam.k3 = k(3); end

    cam.p1 = 0; cam.p2 = 0;
    if numel(p) >= 1, cam.p1 = p(1); end
    if numel(p) >= 2, cam.p2 = p(2); end

    % Image size [height width]
    cam.h = cp.ImageSize(1);
    cam.w = cp.ImageSize(2);
end

