Median + Vectorisation

#include <algorithm>
#include <intrin.h>
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <ctime>
#include <cstring>
#include <omp.h>
#include <xmmintrin.h>
#include <immintrin.h>
#include "lodepng.cpp"
#include "lodepng.h"


using namespace std;


//@@@@@@@@@@@@@@@@@@@@      Vectorisation Filter     @@@@@@@@@@@@@@@@@@@@


unsigned char getVectorisationMedian(std::vector<unsigned char>& values) {
    std::sort(values.begin(), values.end());
    return values[values.size() / 2];
}

void applyVectorisationMedianFilter(std::vector<unsigned char>& image, int width, int height, int channels, int filterSize) {
    std::vector<unsigned char> result(image.size());

    int radius = filterSize / 2;
    const int vecSize = 16; // vector size in bytes

    for (int y = 0; y < height; y++) {
        for (int x = 0; x < width; x++) {
            for (int c = 0; c < channels; c++) {
                std::vector<unsigned char> values;

                // load input values into a vector register
                __m128i inputVec = _mm_setzero_si128();

                for (int i = -radius; i <= radius; i++) {
                    for (int j = -radius; j <= radius; j += vecSize) {
                        int px = std::min(std::max(x + j, 0), width - 1);
                        int py = std::min(std::max(y + i, 0), height - 1);

                        // load vector of input values
                        __m128i curVec = _mm_loadu_si128((__m128i*) & image[(py * width + px) * channels + c]);

                        // merge vectors into a single vector
                        inputVec = _mm_or_si128(inputVec, curVec);
                    }
                }

                // extract values from the vector register
                unsigned char inputArr[vecSize];
                _mm_storeu_si128((__m128i*)inputArr, inputVec);

                // add values to the vector of values
                for (int i = 0; i < vecSize; i++) {
                    values.push_back(inputArr[i]);
                }

                // calculate median and store in result
                result[(y * width + x) * channels + c] = getMedian(values);
            }
        }
    }

    image = result;
}

void medianVectorisationFilter(const char* inputFilename, const char* outputFilename, int filterSize) {
    std::vector<unsigned char> image;
    unsigned width, height;

    // Загружаем изображение
    unsigned error = lodepng::decode(image, width, height, inputFilename);

    if (error) {
        std::cerr << "Error decoding image: " << lodepng_error_text(error) << std::endl;
        return;
    }

    // Применяем медианный фильтр
    applyVectorisationMedianFilter(image, width, height, 4, filterSize);

    // Сохраняем изображение
    error = lodepng::encode(outputFilename, image, width, height);

    if (error) {
        std::cerr << "Error encoding image: " << lodepng_error_text(error) << std::endl;
        return;
    }
}