#include "Filter.h"

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

ClassImp(Filter);

void Filter::DrawBox(const int x1, const int y1,
                     const int x2, const int y2,
                     byte *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, byte *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 byte *buffer, const int offset)
{
    double mean = 0.0;

    byte *s  = (byte*)buffer;
    const byte *e0 = s+768*576;

    //
    // calculate mean value
    //
    while (s<e0)
    {
        const byte *e = s+576-offset;
        s += offset;

        while (s<e)
            mean += *s++;

        s+=offset;
    }

    return mean / ((768-2*offset)*(576-2*offset));
}


float Filter::SDev(const byte *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 - buffer[y*768+x];

            sdev += val*val;
        }

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

    return sqrt(sdev);
}

int Filter::GetMeanPosition(const byte *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 byte m = 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(byte *img)
{
    const int offset = 10;

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

    const byte cut = mean+2.5*sdev>254 ? 254 : (byte)(mean + 2.5*sdev);

    //
    // clean image from noise
    //
    const byte *e = img+768*576;
    byte *i = img;
    while (i<e)
    {
        if (*i<=cut)
            *i = 0;
        i++;
    }

    //
    // 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;

    byte 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];
    }
}