source: trunk/MagicSoft/Cosy/videodev/FilterLed.cc@ 3192

#include "FilterLed.h"

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

#include "Led.h"
#include "Leds.h"
#include "Ring.h"

ClassImp(FilterLed);

void FilterLed::DrawCircle(float cx, float cy, float rpix, byte col) const
{
    if (cx-rpix<1 || cy-rpix<1 || cx+rpix>fW-2 || cy+rpix>fH-2)
        return;

    for (int dx=-(int)(rpix*0.7); dx<(int)(rpix*0.7); dx++)
    {
        const int dy = (int)sqrt(rpix*rpix-dx*dx);
        fImg[(int)(cx+dx) + (int)(cy-dy)*fW] = col;
        fImg[(int)(cx+dx) + (int)(cy+dy)*fW] = col;
        fImg[(int)(cx-dy) + (int)(cy+dx)*fW] = col;
        fImg[(int)(cx+dy) + (int)(cy+dx)*fW] = col;
    }
}

void FilterLed::DrawCircle(const Ring &l, byte col=0x40) const
{
    DrawCircle(l.GetX(), l.GetY(), l.GetR(), col);
}

void FilterLed::DrawCircle(const Ring &l, double r, byte col=0x40) const
{
    DrawCircle(l.GetX(), l.GetY(), r, col);
}

void FilterLed::DrawBox(const int x1, const int y1,
                        const int x2, const int y2,
                        const int col) const
{
    for (int x=x1; x<x2+1; x++)
        for (int y=y1; y<y2+1; y++)
            if (x>=0 && x<fW-1 && y>=0 && y<fH-1)
                fImg[y*fW+x] = col;
}

void FilterLed::MarkPoint(const Led &led) const
{
    const int x = (int)(led.GetX()+.5);
    const int y = (int)(led.GetY()+.5);
    const int m = (int)(led.GetMag());

    MarkPoint(x, y, m);
}

void FilterLed::MarkPoint(Float_t px, Float_t py, Float_t mag) const
{
    const int x = (int)(px+.5);
    const int y = (int)(py+.5);
    const int m = (int)(mag);

    DrawBox(x-8, y, x-5, y, m);
    DrawBox(x, y+5, x, y+8, m);
    DrawBox(x+5, y, x+8, y, m);
    DrawBox(x, y-8, x, y-5, m);
    return;
}

void FilterLed::GetMinMax(const int offset, byte *min, byte *max) const
{
    *min = fImg[0];
    *max = fImg[0];

    byte *s = (byte*)fImg;
    const byte *e0 = s+fW*fH;

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

        while (s<e)
        {
            if (*s>*max)
            {
                *max = *s;
                if (*max-*min==255)
                    return;
            }
            if (*s<*min)
            {
                *min = *s;
                if (*max-*min==255)
                    return;
            }
            s++;
        }
        s+=offset;
    }
}

int FilterLed::GetMeanPosition(const int x, const int y,
                               const int box, float &mx, float &my) const
{
    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 = fImg[dy*fW+dx];

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

    mx = (float)sumx/sum;
    my = (float)sumy/sum;

    return (int)my*fW + (int)mx;
}

int FilterLed::GetMeanPosition(const int x, const int y, const int box) const
{
    float mx, my;
    return GetMeanPosition(x, y, box, mx, my);
}
/*
void FilterLed::RemoveTwins(Leds &leds, Double_t radius)
{
    for (int i=first; i<leds.GetEntriesFast(); i++)
    {
        const Led &led1 = leds(i);

        const Double_t x1 = led1.GetX();
        const Double_t y1 = led1.GetY();

        for (int j=first; j<leds.GetEntriesFast(); j++)
        {
            if (j==i)
                continuel

            const Led &led2 = leds(j);

            const Double_t x2 = led2.GetX();
            const Double_t y2 = led2.GetY();

            const Double_t dx = x2-x1;
            const Double_t dy = y2-y1;

            if (dx*dx+dy*dy<radius*radius)
            {
                // FIXME: Interpolation
                leds.Remove(led2);
            }
        }
    }
}
*/
void FilterLed::RemoveTwinsInterpol(Leds &leds, Int_t first, Double_t radius) const
{
    const Int_t num=leds.GetEntriesFast();

    for (int i=first; i<num; i++)
    {
        Led *led1 = (Led*)leds.UncheckedAt(i);
        if (!led1)
            continue;

        const Double_t x1 = led1->GetX();
        const Double_t y1 = led1->GetY();

        Double_t mag = led1->GetMag();
        Double_t x = x1*mag;
        Double_t y = y1*mag;

        Double_t sqm = mag*mag;
        Double_t sqx = x*x;
        Double_t sqy = y*y;

        Int_t n=1;

        for (int j=first; j<num; j++)
        {
            if (i==j)
                continue;

            Led *led2 = (Led*)leds.UncheckedAt(j);
            if (!led2)
                continue;

            Double_t x2 = led2->GetX();
            Double_t y2 = led2->GetY();

            const Double_t dx = x2-x1;
            const Double_t dy = y2-y1;

            if (dx*dx+dy*dy>radius*radius)
                continue;

            // Multiply with weihgt
            const Double_t w = led2->GetMag();
            x2 *= w;
            y2 *= w;

            x   += x2;
            y   += y2;
            mag += w;

            sqx += x2*x2;
            sqy += y2*y2;
            sqm += w*w;

            n++;
            leds.Remove(led2);
        }

        x /= mag;
        y /= mag;

        sqx /= sqm;
        sqy /= sqm;

        leds.Add(x, y, 0/*sqrt(sqx-x*x)*/, 0/*sqrt(sqy-y*y)*/, mag/n);
        leds.Remove(led1);
    }
    leds.Compress();
}

void FilterLed::ExecuteAndMark(Leds &leds, int xc, int yc) const
{
    const Int_t first = leds.GetEntriesFast();

    Execute(leds, xc, yc);

    // Mark Stars in image
    for (int i=first; i<leds.GetEntriesFast(); i++)
        MarkPoint(leds(i));
}

void FilterLed::Execute(int xc, int yc) const
{
    Leds leds;
    ExecuteAndMark(leds, xc, yc);
}

void FilterLed::Execute(Leds &leds, int xc, int yc) const
{
    int x0 = xc-fBoxW;
    int x1 = xc+fBoxW;
    int y0 = yc-fBoxH;
    int y1 = yc+fBoxH;

    if (x0<0) x0=0;
    if (y0<0) y0=0;
    if (x1>fW) x1=fW;
    if (y1>fH) y1=fH;

    const int wx = x1-x0;
    const int hy = y1-y0;

    double sum = 0;
    double sq  = 0;

    for (int x=x0; x<x1; x++)
        for (int y=y0; y<y1; y++)
        {
            byte &b = fImg[y*fW+x];

            sum += b;
            sq  += b*b;
        }

    sum /= wx*hy;
    sq  /= wx*hy;

    const double sdev = sqrt(sq-sum*sum);
    const byte   max  = sum+fCut*sdev>254 ? 254 : (byte)(sum+fCut*sdev);

    //
    // clean image from noise
    // (FIXME: A lookup table could accelerate things...
    //
    for (int x=x0; x<x1; x++)
        for (int y=y0; y<y1; y++)
        {
            byte &b = fImg[y*fW+x];
            if (b<=max)
                b = 0;
        }

    //
    // find mean points
    //
    const int maxpnt = wx*hy>0x4000?0x4000:wx*hy;

    int  pos[maxpnt]; // (Use 'new' instead for big numbers!)
    byte mag[maxpnt]; // (Use 'new' instead for big numbers!)

    int cnt = 0;
    for (int x=x0; x<x1; x++)
        for (int y=y0; y<y1; y++)
        {
            byte &b = fImg[y*fW+x];
            if (b==0)
                continue;

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

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

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

            cnt++;
            if (cnt==0x4000)
                return;
        }

    if (cnt>1000)
        cout << "FIXME: Cnt>1000." << endl;

    //
    // Add found positions to array
    //
    const int first=leds.GetEntriesFast();

    for (int i=0; i<cnt; i++)
    {
        if (mag[i]<=0x80) // 0xa0
            continue;

        Float_t mx, my;
        GetMeanPosition(pos[i]%fW, pos[i]/fW, 5, mx, my);

        leds.Add(mx, my, 0, 0, mag[i]);
    }

    RemoveTwinsInterpol(leds, first, 5);
}

void FilterLed::Stretch() const
{
    byte min, max;
    GetMinMax(25, &min, &max);

    if (min==max || max-min>230) // 255/230=1.1
        return;

    const float scale = 255./(max-min);

    byte *b = fImg;
    const byte *e = fImg+fW*fH;

    while (b<e)
    {
        if (*b<min)
        {
            *b++=0;
            continue;
        }
        if (*b>max)
        {
            *b++=255;
            continue;
        }
        *b = (byte)((*b-min)*scale);
        b++;
    }
}