#include "CaosFilter.h"

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

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

ClassImp(CaosFilter);

void CaosFilter::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 CaosFilter::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;
}

void CaosFilter::GetStat(const byte *buffer, const int offset, double *mean, double *sdev)
{
    double sum = 0;
    double sq  = 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)
        {
            sum += *s;
            sq  += *s * *s;
            s++;
        }

        s+=offset;
    }

    const Int_t sz = (768-2*offset)*(576-2*offset);

    sum /= sz;
    sq  /= sz;

    *mean = sum;
    *sdev = sqrt(sq-sum*sum);
}

int CaosFilter::GetMeanPosition(const byte *bitmap, const int x, const int y,
                                const int box, Float_t &mx, Float_t &my)
{
    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;
        }

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

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

int CaosFilter::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;
}


//
// Calculation of center of rings
//
Double_t sqr(Double_t x) { return x*x; }

void swap(int *m, int *n)
{
    int dummy = *m;
    *m = *n;
    *n = dummy;
}

bool CaosFilter::CalcCenter(Float_t *px, Float_t *py, Float_t &cx,
                            Float_t &cy, Float_t &R)
{
    int i=0;
    int j=1;
    int k=2;

    Float_t h1 = py[i]-py[j];

    if (h1==0)
    {
        swap(&j, &k);
        h1 = py[i]-py[j];
        if (h1==0)
        {
            cout << "h1==0" <<endl;
            return kFALSE;
        }
    }

    Float_t h2 = py[j]-py[k];

    if (h2==0)
    {
        swap(&i, &j);
        h2 = py[j]-py[k];
        if (h2==0)
        {
            cout << "h2==0" << endl;
            return kFALSE;
        }
    }

    Float_t w1 = px[i]-px[j];
    Float_t w2 = px[j]-px[k];

    Float_t m1 = -w1/h1;
    Float_t m2 = -w2/h2;

    if (m2-m1==0)
    {
        cout << "m2-m1==0" << endl;
        return kFALSE;
    }

    cx = ((m2*(px[j]+px[k]) +py[i]-py[k]        -m1*(px[i]+px[j]))/(m2-m1)/2);
    cy = ((m2*(py[i]+py[j]) +m1*m2*(px[k]-px[i])-m1*(py[j]+py[k]))/(m2-m1)/2);

    R = sqrt(sqr(cx-px[i])+sqr(cy-py[i]));

    return kTRUE;
}

//
// Interpolation of centers of rings
//
void CaosFilter::InterpolCenter(int &m, Float_t *x, Float_t *y, Float_t &px,
                                Float_t &py, Float_t &pr, Float_t &sx,
                                Float_t &sy, Float_t &sr)
{
    int nPoints = m;
    int mn[10]={0,0,0,1,4,10,20,35,56,84};
    int nRings = mn[m];
    Float_t rx[10];
    Float_t ry[10];
    Float_t rr[10];
    px = 0;
    py = 0;
    pr = 0;

    int n=0;
    for (int i=0; i<nPoints-2; i++)
        for (int j=i+1; j<nPoints-1; j++)
            for (int k=j+1; k<nPoints; k++)
            {
                rx[n]=0;
                ry[n]=0;
                rr[n]=0;

                Float_t xx[3] = { x[i], x[j], x[k] };
                Float_t yy[3] = { y[i], y[j], y[k] };
                CalcCenter(xx, yy, rx[n], ry[n], rr[n]);

                px += rx[n];
                py += ry[n];
                pr += rr[n];

                n++;
            }

    px /= n;
    py /= n;
    pr /= n;

    //
    // deviation of x- and y coordinate and radius
    //
    Float_t sumx=0;
    Float_t sumy=0;
    Float_t sumr=0;

    for (n=0; n<nRings; n++)
    {
        sumx += sqr(rx[n]-px);
        sumy += sqr(ry[n]-py);
        sumr += sqr(rr[n]-pr);

    }
    sx=sqrt(sumx)/(nRings-1);
    sy=sqrt(sumy)/(nRings-1);
    sr=sqrt(sumr)/(nRings-1);
}



//
//Calculation of rings
//

Double_t kConv = 0.502; // [pix/mm]

void CaosFilter::CalcRings(int &m, Float_t *x, Float_t *y, Float_t &px,
                       Float_t &py, Float_t &pr, float *v, float *w)
{

//    Float_t v[5];
//    Float_t w[5];
    Float_t s[5];
    Float_t sx;
    Float_t sy;
    Float_t sr;

    for (int i=0; i<6; i++)
    {
        x[i] /= kConv;
        y[i] /= kConv;
    }

    InterpolCenter(m, x, y, px, py, pr, sx, sy, sr);

    //
    // angles v and relative angle w
    //
    for (int j=0; j<6; j++)
    {
        v[j] = atan2(y[j]-py, x[j]-px)*180/TMath::Pi();
    }

    w[0]=v[0]+167.11;
    w[1]=v[1]-94.31;
    w[2]=v[2]+87;
    w[3]=v[3]+56.58;
    w[4]=v[4]-33.48;

    //
    // distances between the LEDs
    //
    s[0]=sqrt(sqr(x[1]-x[0]) + sqr(y[1]-y[0]));///41.6;
    s[1]=sqrt(sqr(x[1]-x[4]) + sqr(y[1]-y[4]));///27.7;
    s[2]=sqrt(sqr(x[3]-x[4]) + sqr(y[3]-y[4]));///39.1;
    s[3]=sqrt(sqr(x[2]-x[3]) + sqr(y[2]-y[3]));///14.4;
    s[4]=sqrt(sqr(x[2]-x[0]) + sqr(y[2]-y[0]));///35.2;

}

//
// filter LEDs from spots
//
int CaosFilter::FilterLeds(float *xw, float *yw, float *xl, float *yl)
{
    int n=0;
    int m=0;

    for (n=0; n<10; n++)
    {
        if (!(xw[n]>250 && xw[n]<540 && yw[n]>140 && yw[n]<500))
            continue;

        if (n>0 && xw[n]>(xl[m-1]-3) && xw[n]<(xl[m-1]+3))
            continue;

        xl[m]=xw[n];
        yl[m]=yw[n];
        m++;
    }
    return m;
}

void CaosFilter::FilterLeds(Leds &leds)
{
    for (int n=0; n<leds.GetEntries(); n++)
    {
        const Led &led = leds(n);

        const Double_t x = led.GetX()-480;
        const Double_t y = led.GetY()-200;

        const Double_t r2 = x*x+y*y;

        if (r2<80*80 || r2>145*145)
        {
            leds.RemoveAt(n);
            continue;
        }
    }

    RemoveTwins(leds, 5);
}

void CaosFilter::RemoveTwins(Leds &leds, Double_t radius)
{
    TIter Next1(&leds);

    Led *led1 = NULL;

    while ((led1=(Led*)Next1()))
    {
        Led *led2 = NULL;
        TIter Next2(&leds);

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

        while ((led2=(Led*)Next2()))
        {
            if (led1==led2)
                continue;

            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 CaosFilter::Execute(byte *img, float *xw, float *yw, float *xl,
                         float *yl, float &prx, float &pry, float &pr, float *v, float *w)
{
    const int offset = 10;

    double mean, sdev;
    GetStat(img, offset, &mean, &sdev);

    const byte max = 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<=max)
            *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; // pixel number
            pos[cnt][1] = 0x10; // magnitude

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

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

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

        Float_t mx, my;
        GetMeanPosition(img, px, py, 5, mx, my);

        //
        // Write positions in array
        //
        //            cout << mx << "  " << my << "  " << cnt << endl;
        //            cout << "points:" << points << endl;

        xw[points]=mx;
        yw[points]=my;

        points++;
    }

    int m = FilterLeds(xw, yw, xl, yl);
    CalcRings(m, xl, yl, prx, pry, pr, v, w);

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

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

void CaosFilter::Execute(byte *img, Leds &leds, Double_t conv)
{
    const int offset = 10;

    double mean, sdev;
    GetStat(img, offset, &mean, &sdev);

    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; // pixel number
            pos[cnt][1] = 0x10; // magnitude

            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]<=0x80) // 0xa0
            continue;

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

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

        Float_t mx, my;
        GetMeanPosition(img, px, py, 5, mx, my);

        //
        // Write positions in array
        //
        //            cout << mx << "  " << my << "  " << cnt << endl;
        //            cout << "points:" << points << endl;

        leds.Set(points++, mx/conv, my/conv, 0, 0, pos[i][1]);
    }

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

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

        
