#include <stdlib.h>
#include "filter.h"


void filter_highpass_3x3( double *output,
                          double *i0, double *i1, double *i2,
                          int width )
{
    int i;

    if( width < 2 ) {
        *output = 0.0;
        return;
    }

    /* handle first pixel */
    *output++ = -i0[ 0 ] - i0[ 1 ]
                + (8.0 * i1[ 0 ]) - i1[ 1 ]
                - i2[ 0 ] - i2[ 1 ];

    for( i = 1; i < width - 1; i++ ) {
        *output++ =  -i0[ i - 1 ] - i0[ i ] - i0[ i + 1 ]
                    - i1[ i - 1 ] + (8.0 * i1[ i ]) - i1[ i + 1 ]
                    - i2[ i - 1 ] - i2[ i ] - i2[ i + 1 ];
    }

    /* handle last pixel */
    *output++ = -i0[ width - 2 ] - i0[ width - 1 ]
                -i1[ width - 2 ] + (8.0 * i1[ width - 1 ])
                - i2[ width - 2 ] - i2[ width - 1 ];
}

void filter3_highpass_3x3( double *output,
                           double *i0, double *i1, double *i2,
                           int width )
{
    int i;

    if( width < 2 ) {
        *output++ = 0.0;
        *output++ = 0.0;
        *output   = 0.0;
        return;
    }

    /* handle first pixel */
    *output++ = -i0[ 0 ] - i0[ 3 ]
                + (8.0 * i1[ 0 ]) - i1[ 3 ]
                - i2[ 0 ] - i2[ 3 ];
    *output++ = -i0[ 1 ] - i0[ 4 ]
                + (8.0 * i1[ 1 ]) - i1[ 4 ]
                - i2[ 1 ] - i2[ 4 ];
    *output++ = -i0[ 2 ] - i0[ 5 ]
                + (8.0 * i1[ 2 ]) - i1[ 5 ]
                - i2[ 2 ] - i2[ 5 ];

    width = width * 3;
    for( i = 3; i < width - 3; i += 3 ) {
        *output++ =  -i0[ i - 3 ] - i0[ i ] - i0[ i + 3 ]
                    - i1[ i - 3 ] + (8.0 * i1[ i ]) - i1[ i + 3 ]
                    - i2[ i - 3 ] - i2[ i ] - i2[ i + 3 ];
        *output++ =  -i0[ i - 2 ] - i0[ i + 1 ] - i0[ i + 4 ]
                    - i1[ i - 2 ] + (8.0 * i1[ i + 1 ]) - i1[ i + 4 ]
                    - i2[ i - 2 ] - i2[ i + 1 ] - i2[ i + 4 ];
        *output++ =  -i0[ i - 1 ] - i0[ i + 2 ] - i0[ i + 5 ]
                    - i1[ i - 1 ] + (8.0 * i1[ i + 2 ]) - i1[ i + 5 ]
                    - i2[ i - 1 ] - i2[ i + 2 ] - i2[ i + 5 ];
    }

    /* handle last pixel */
    *output++ = -i0[ width - 6 ] - i0[ width - 3 ]
                -i1[ width - 6 ] + (8.0 * i1[ width - 3 ])
                - i2[ width - 6 ] - i2[ width - 3 ];
    *output++ = -i0[ width - 5 ] - i0[ width - 2 ]
                -i1[ width - 5 ] + (8.0 * i1[ width - 2 ])
                - i2[ width - 5 ] - i2[ width - 2 ];
    *output++ = -i0[ width - 4 ] - i0[ width - 1 ]
                -i1[ width - 4 ] + (8.0 * i1[ width - 1 ])
                - i2[ width - 4 ] - i2[ width - 1 ];
}

void image3_highpass( double *output,
                      double *input, int width, int height )
{
    double *zeros = (double *) malloc( width * 3 * sizeof( double ) );
    int stride = width * 3;
    int i;

    for( i = 0; i < width * 3; i++ ) zeros[ i ] = 0.0;

    filter3_highpass_3x3( output, zeros, input, input + stride, width );

    for( i = 1; i < height - 1; i++ ) {
        filter3_highpass_3x3( output + (i * stride),
                              input + ((i-1) * stride),
                              input + (i * stride),
                              input + ((i+1) * stride), width );
    }

    filter3_highpass_3x3( output + ((height-1)*stride),
                          input + ((height-2)*stride),
                          input + ((height-1)*stride),
                          zeros, width );

    free( zeros );
}