#include "stdint.h"
#include "math.h"

#include <memory>
#include <algorithm>

#define ELEMENT_AT( ARRAY, ROW, COLUMN ) ARRAY[( ROW ) * ( width ) + ( COLUMN )]

const double numerator      = 255.;
const double denonimator    = 1. / 255.;
const double sqrt2          = 1.41421356237;

void createImageDoubleArray( const uint8_t* const img, int width, int height, double* outImage )
{
    for ( int i = 0, size = width * height; i < size; ++i )
    {
        if ( img[i] == 255 )
            outImage[i] = 1.;
        else
        {
            if ( img[i] == 0 )
                outImage[i] = 0.;
            else
                outImage[i] = static_cast<double>( img[i] ) * denonimator;
        }
    }
}

void createImageUintArray( const double* const img, int width, int height, uint8_t* outImage )
{
    for ( int i = 0, size = width * height; i < size; ++i )
    {
        if ( img[i] == 1. )
            outImage[i] = 255;
        else
        {
            if ( img[i] == 0. )
                outImage[i] = 0;
            else
                outImage[i] = static_cast<uint8_t>( static_cast<double>( img[i] ) * numerator );
        }
    }
}

void calculateImageGradient( const double* const img, int width, int height, double* outGradientX, double* outGradientY )
{
    for ( int rowIndex = 1; rowIndex < height - 1; ++rowIndex )
    {
        for ( int columnIndex = 1; columnIndex < width - 1; ++columnIndex )
        {
            // Вычисляем градиент только в местах контура ( где должно соблюдаться сглаживание )
            if ( 0.0 < ELEMENT_AT( img, rowIndex, columnIndex ) && ELEMENT_AT( img, rowIndex, columnIndex) < 1.0 )
            {
                ELEMENT_AT( outGradientX, rowIndex, columnIndex ) = -ELEMENT_AT( img, rowIndex - 1, columnIndex - 1 ) - sqrt2 *
                                                                     ELEMENT_AT( img, rowIndex    , columnIndex - 1 ) -
                                                                     ELEMENT_AT( img, rowIndex + 1, columnIndex - 1 ) +
                                                                     ELEMENT_AT( img, rowIndex - 1, columnIndex + 1 ) + sqrt2 *
                                                                     ELEMENT_AT( img, rowIndex    , columnIndex + 1 ) +
                                                                     ELEMENT_AT( img, rowIndex + 1, columnIndex + 1 ) ;

                ELEMENT_AT( outGradientY, rowIndex, columnIndex ) = -ELEMENT_AT( img, rowIndex - 1, columnIndex - 1 ) - sqrt2 *
                                                                     ELEMENT_AT( img, rowIndex - 1, columnIndex     ) -
                                                                     ELEMENT_AT( img, rowIndex - 1, columnIndex + 1 ) +
                                                                     ELEMENT_AT( img, rowIndex + 1, columnIndex - 1 ) + sqrt2 *
                                                                     ELEMENT_AT( img, rowIndex + 1, columnIndex     ) +
                                                                     ELEMENT_AT( img, rowIndex + 1, columnIndex + 1 ) ;

                const double length2 = ELEMENT_AT( outGradientX, rowIndex, columnIndex ) * ELEMENT_AT( outGradientX, rowIndex, columnIndex ) +
                                       ELEMENT_AT( outGradientY, rowIndex, columnIndex ) * ELEMENT_AT( outGradientY, rowIndex, columnIndex );

                // Процесс нормализации
                if ( length2 > 0.0 )
                {
                    const double lengthInverse = 1.0 / sqrt( length2 );

                    ELEMENT_AT( outGradientX, rowIndex, columnIndex ) *= lengthInverse;
                    ELEMENT_AT( outGradientY, rowIndex, columnIndex ) *= lengthInverse;
                }
            }
        }
    }
}

double calculateEdgeDistanceFunction( double gradientX, double gradientY, double a )
{
    if ( ( gradientX < 1e-12 ) || ( gradientY < 1e-12 ) )
        return 0.5 - a;
    else
    {
        gradientX = fabs( gradientX );
        gradientY = fabs( gradientY );

        if ( gradientX < gradientY )
            std::swap( gradientX, gradientY );

        const double a1 = 0.5 * gradientY / gradientX;

        // 0 <= a < a1
        if ( a < a1 )
            return 0.5 * ( gradientX + gradientY ) - sqrt( 2.0 * gradientX * gradientY * a );
        else
        {
            // a1 <= a <= 1 - a1
            if ( a < ( 1.0 - a1 ) )
                return ( 0.5 - a ) * gradientX;
            else
                return -0.5 * ( gradientX + gradientY ) + sqrt( 2.0 * gradientX * gradientY * ( 1.0 - a ) );
        }
    }
}

double calculatDistanceAA( double imageValue, double gradientX, double gradientY, double dx, double dy )
{
    if ( imageValue < 1e-12 )
        return 1e9;

    const double di = sqrt( dx * dx + dy * dy );

    if ( di < 1e-12 )
        return di + calculateEdgeDistanceFunction( gradientX, gradientY, imageValue );
    else
        return di + calculateEdgeDistanceFunction( dx, dy, imageValue );
}

#define EQUATE_VALUES()                                             \
                    {                                               \
                        distanceX[index]        = newDistanceX;     \
                        distanceY[index]        = newDistanceY;     \
                        outDistanceImage[index] = newDistanceImage; \
                        oldDistance             = newDistanceImage; \
                        isChanged               = true;             \
                    }                                               \

void startFilter( const double* const img, const double* const gradientX, const double* const gradientY, int width, int height, double* outDistanceImage )
{
    const int size = width * height;

    auto distanceX = std::make_unique<int[]>( size );
    auto distanceY = std::make_unique<int[]>( size );

    for ( int i = 0; i < size; ++i )
    {
        distanceX[i] = 0;
        distanceX[i] = 0;

        if ( img[i] <= 0.0 )
            outDistanceImage[i] = 1e9;
        else
        {
            if ( img[i] < 1.0 )
                outDistanceImage[i] = calculateEdgeDistanceFunction( gradientX[i], gradientY[i], img[i] );
            else
                outDistanceImage[i] = 0.0;
        }
    }

    const int offset_u  = -width;
    const int offset_ur = -width + 1;
    const int offset_r  = +1;
    const int offset_rd = +width + 1;
    const int offset_d  = +width;
    const int offset_dl = +width - 1;
    const int offset_l  = -1;
    const int offset_lu = -width - 1;

    for ( bool isChanged = false; ; isChanged = false )
    {
        for ( int row = 1; row < height; ++row )
        {
            int     index       = row * width;

            double  oldDistance = outDistanceImage[index];

            if ( oldDistance > 0.0 )
            {
                {
                    const int c = index + offset_u;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c];
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_ur;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            }

            ++index;

            for ( int column = 1; column < width - 1; ++column, ++index )
            {
                oldDistance = outDistanceImage[index];

                if ( oldDistance <= 1e-12 )
                    continue;

                {
                    const int c = index + offset_l;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c];

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_lu;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_u;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c];
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_ur;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            } // end for

            oldDistance = outDistanceImage[index];

            if ( oldDistance > 0.0 )
            {
                {
                    const int c = index + offset_l;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c];

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_lu;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_u;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c];
                    const int newDistanceY = distanceY[c] + 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            } // end if

            index = ( row + 1 ) * width - 2;

            for ( int column = width - 2; column >= 0; --column, --index )
            {
                oldDistance = outDistanceImage[index];

                if ( oldDistance <= 1e-12 )
                    continue;

                {
                    const int c = index + offset_r;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c];

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            }
        } // end row for

        for ( int row = height - 2;  row >= 0; --row )
        {
            int     index       = ( row + 1 ) * width - 1;
            double  oldDistance = outDistanceImage[index];

            if ( oldDistance > 0.0 )
            {
                {
                    const int c = index + offset_d;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c];
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_dl;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            } // end if

            --index;

            for ( int column = width - 2; width > 0; --column, --index )
            {
                oldDistance = outDistanceImage[index];

                if ( oldDistance <= 1e-12 )
                    continue;

                {
                    const int c = index + offset_r;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c];

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_rd;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_d;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c];
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_dl;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            } // end for

            oldDistance = outDistanceImage[index];

            if ( oldDistance > 0.0 )
            {
                {
                    const int c = index + offset_r;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c];

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_rd;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] - 1;
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }

                {
                    const int c = index + offset_d;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c];
                    const int newDistanceY = distanceY[c] - 1;

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            } // end if

            index = row * width + 1;

            for ( int column = 1; column < width; ++column, ++index )
            {
                oldDistance = outDistanceImage[index];

                if ( oldDistance <= 1e-12 )
                    continue;

                {
                    const int c = index + offset_l;

                    const int closestIndex = c - distanceX[c] - distanceY[c] * width;

                    const int newDistanceX = distanceX[c] + 1;
                    const int newDistanceY = distanceY[c];

                    const double newDistanceImage =
                        calculatDistanceAA( img[closestIndex], gradientX[closestIndex], gradientY[closestIndex], newDistanceX, newDistanceY);

                    if ( newDistanceImage < oldDistance - 1e-6 )
                        EQUATE_VALUES();
                }
            }
        }

        if ( !isChanged )
            return;
    }
}

void createSDF( const uint8_t* const img, int width, int height, uint8_t* outImage)
{
    const int size = width * height;

    std::unique_ptr<double[]> doubleGrayScaleImage = std::make_unique<double[]>( size );
    createImageDoubleArray( img, width, height, doubleGrayScaleImage.get() );

    std::unique_ptr<double[]> dgradientX = std::make_unique<double[]>( size );
    std::unique_ptr<double[]> dgradientY = std::make_unique<double[]>( size );

    calculateImageGradient( doubleGrayScaleImage.get(), width, height, dgradientX.get(), dgradientY.get() );

    std::unique_ptr<double[]> doubleOutImage = std::make_unique<double[]>( size );
    startFilter( doubleGrayScaleImage.get(), dgradientX.get(), dgradientY.get(), width, height, doubleOutImage.get() );

    createImageUintArray( doubleOutImage.get(), width, height, outImage );
}

int main()
{
    // TEST( testSDFGenerator() );
}