#include "Filter.h"

#include <memory.h>   // memset
#include <iostream.h> // cout

void Filter::DrawBox(const int x1, const int y1,
                     const int x2, const int y2,
                     char *buffer, const int col)
{
    for (int x=x1; x<x2+1; x++)
        for (int y=y1; y<y2+1; y++)
            buffer[y*768+x] = col;
}

void Filter::MarkPoint(const int x, const int y, char *buffer, const int col)
{
    DrawBox(x-8, y, x-5, y, buffer, col);
    DrawBox(x, y+5, x, y+8, buffer, col);
    DrawBox(x+5, y, x+8, y, buffer, col);
    DrawBox(x, y-8, x, y-5, buffer, col);
    return;
}

float Filter::Mean(const char *buffer, const int offset, int *min, int *max)
{
    double mean = 0.0;

    *min = 0xff;
    *max = 0x00;

    //
    // calculate mean value
    //
    for (int x=offset; x<768-offset; x++)
        for (int y=offset; y<576-offset; y++)
        {
            unsigned char val = (unsigned char)buffer[y*768+x];

            mean += val;

            if (*max<val)
                *max = val;

            if (*min>val)
                *min = val;
        }

    mean /= (768-2*offset)*(576-2*offset);

    return mean;
}

float Filter::SDev(const char *buffer, const int offset, const double mean)
{
    //
    // calculate sigma
    //
    double sdev=0.0;

    for (int x=offset; x<768-offset; x++)
        for (int y=offset; y<576-offset; y++)
        {
            const float val = mean - (unsigned char)buffer[y*768+x];

            sdev += val*val;
        }

    sdev /= (768-2*offset)*(576-2*offset)-1;

    return sqrt(sdev);
}

int Filter::GetMeanPosition(const char *bitmap, const int x, const int y,
                            const int box)
{
    unsigned int sumx=0;
    unsigned int sumy=0;

    unsigned int sum=0;

    for (int dx=x-box; dx<x+box+1; dx++)
        for (int dy=y-box; dy<y+box+1; dy++)
        {
            const unsigned char m = (unsigned char)bitmap[dy*768+dx]; // desc->buffer[3*(x+y*768)]; //

            sumx += m*dx;
            sumy += m*dy;
            sum  += m;
        }

    const float px = (float)sumx/sum;
    const float py = (float)sumy/sum;

    return (int)py*768 + (int)px;
}

void Filter::Execute(char *img)
{
    const int offset = 10;

    int max;
    int min;

    const float mean = Mean(img, offset, &min, &max);
    const float sdev = SDev(img, offset, mean);

    const float cut = mean + 3*sdev;

    //
    // clean image from noise
    //
    for (int x=0; x<768; x++)
        for (int y=0; y<576; y++)
        {
            if (img[y*768+x]>cut)
                continue;

            img[y*768+x] = 0;
        }

    //
    // find mean points
    //
    const int maxpnt = 0x1000;

    int pos[maxpnt+1][2]; // FIXME
    int cnt = 0;

    for (int x=offset; x<768-offset; x++)
    {
        for (int y=offset; y<576-offset; y++)
        {
            if (img[x+768*y]==0)
                continue;

            const int ipos = GetMeanPosition(img, x, y, 5);

            int j;
            for (j=0; j<cnt; j++)
            {
                if (pos[j][0]==ipos)
                {
                    if (pos[j][1] < 0xf0)
                        pos[j][1] += 0x10;
                    break;
                }
            }
            if (cnt && j<cnt)
                continue;

            pos[cnt][0] = ipos;
            pos[cnt][1] = 0x10;

            cnt++;

            if (cnt==maxpnt)
                break;
        }
        if (cnt==maxpnt)
        {
            cout << "Error! More than " << maxpnt << " stars found." << endl;
            break;
        }
    }

    //
    // Draw marker for found stars into picture
    //
    int points=0;

    char marker[768*576];
    memset(marker, 0, 768*576);

    for (int i=0; i<cnt; i++)
    {
        if (pos[i][1]>0xa0)
        {
            points++;

            int px = pos[i][0]%768;
            int py = pos[i][0]/768;

            MarkPoint(px, py, marker, pos[i][1]);
        }
    }

    //
    // Copy markers into image
    //
    for (int x=0; x<768*576; x++)
    {
        if (!marker[x])
            continue;

        img[x]=marker[x];
    }
}

