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

#include "FilterLed.h"

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

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

#include "MGMap.h"

ClassImp(FilterLed);

void FilterLed::DrawBox(const int x1, const int y1,
                        const int x2, const int y2,
                        const int col) const
{
    MGMap::DrawBox(fImg, 768, 576, x1, y1, x2, y2, col);
}

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);
}

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::DrawCircle(float cx, float cy, float r, byte col) const
{
    MGMap::DrawCircle(fImg, 768, 576, cx, cy, r, col);
}

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

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

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 (speed optimized)
    //
    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, unsigned int &sum) const
{
    unsigned int sumx=0;
    unsigned int sumy=0;

    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;
    unsigned int sum;
    return GetMeanPosition(x, y, box, mx, my, sum);
}
/*
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::ExecuteAndMark(Leds &leds, int xc, int yc, double &bright) const
{
    const Int_t first = leds.GetEntriesFast();

    Execute(leds, xc, yc, bright);
 
    // 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
{
    double bright;
    Execute(leds, xc, yc, bright);
}


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

    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;

    bright=sum;

    
    // 254 because b<=max and not b<max
    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;
        unsigned int sum;
        GetMeanPosition(pos[i]%fW, pos[i]/fW, 5, mx, my, sum);

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

    RemoveTwinsInterpol(leds, first, 5);
    
   
}

void FilterLed::FindStar(Leds &leds, int xc, int yc) const
{
    // fBox: radius of the inner (signal) box
    // Radius of the outer box is fBox*sqrt(2)

    //
    // Define inner box in which to search the signal
    //
    int x0 = xc-fBox;
    int x1 = xc+fBox;
    int y0 = yc-fBox;
    int y1 = yc+fBox;

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

    //
    // Define outer box (excluding inner box) having almost
    // the same number of pixels in which the background
    // is calculated
    //
    const double sqrt2 = sqrt(2.);

    int xa = xc-(int)rint(fBox*sqrt2);
    int xb = xc+(int)rint(fBox*sqrt2);
    int ya = yc-(int)rint(fBox*sqrt2);
    int yb = yc+(int)rint(fBox*sqrt2);

    if (xa<0) xa=0;
    if (ya<0) ya=0;
    if (xb>fW) xb=fW;
    if (yb>fH) yb=fH;

    //
    // Calculate average and sdev for a square
    // excluding the inner part were we expect
    // the signal to be.
    //
    double sum = 0;
    double sq  = 0;

    int n=0;
    for (int x=xa; x<xb; x++)
        for (int y=ya; y<yb; y++)
        {
            if (x>=x0 && x<x1 && y>=y0 && y<y1)
                continue;

            byte &b = fImg[y*fW+x];

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

    sum /= n;
    sq  /= n;

    // 254 because b<=max and not b<max
    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...
    //
    n=0;
    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;
            else
                n++;
        }

    //
    // Mark the background region
    //
    for (int x=xa; x<xb; x+=2)
    {
        fImg[ya*fW+x]=0xf0;
        fImg[yb*fW+x]=0xf0;
    }
    for (int y=ya; y<yb; y+=2)
    {
        fImg[y*fW+xa]=0xf0;
        fImg[y*fW+xb]=0xf0;
    }

    //
    // Check if any pixel found...
    //
    if (n<5)
        return;

    //
    // Get the mean position of the star
    //
    float mx, my;
    unsigned int mag;
    int pos = GetMeanPosition(xc, yc, fBox-1, mx, my, mag);

    if (pos<0 || pos>=fW*fH && fImg[pos]<sum+fCut*sdev)
        return;

    cout << "Mean=" << sum << "  SDev=" << sdev << "  :  ";
    cout << "Sum/n = " << sum << "/" << n << " = " << (n==0?0:mag/n) << endl;

    leds.Add(mx, my, 0, 0, -2.5*log10((float)mag)+13.7);
}

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++;
    }
}