#include "scsimulator.h"

#include <QImage>
#include <QPainter>

class ScSimulator::Implementation
{
public:
    QImage* testPattern;

    float sweep_data[4096];

    float cx;
    float cy;

    void tstImageCreate()
    {
        QVector<QRgb> ptab(256, 0);
        for(int i=0; i<256; ++i)
            ptab[i]=qRgb(i, i, i);

        QImage tmp(":/images/images/sc_test_image.bmp");
        QImage tmp2=tmp.mirrored(true, true);
        QImage tmp1=tmp2.scaled(QSize(1024, 1024), Qt::IgnoreAspectRatio, Qt::SmoothTransformation);

        testPattern=new QImage(1024, 1024, QImage::Format_Indexed8);
        testPattern->setColorTable(ptab);

        int nr=tmp1.height();
        int nc=tmp1.width();
        for(int r=0; r<nr; ++r)
            for(int c=0; c<nc; ++c)
            {
                int gray=qGray(tmp1.pixel(c, r));
                testPattern->setPixel(c, r, gray); //QColor(gray, gray, gray).rgb());
            }

        //cx=testPattern->width()/2;
        //cy=cx;
        reset();
    }

    void reset()
    {
        if (!testPattern)
            tstImageCreate();
        cx=testPattern->width()/2;
        cy=testPattern->height()-1; //p_.cx;
    }

    int irnd(float x)
    {
        return x<0 ? x-0.5 : x+0.5;
    }

    const float* inverseSC(float delta_t, float vx, float vy, float angle, unsigned int rbins=2048)
    {
        if (!testPattern)
            tstImageCreate();

        QImage* tp=testPattern;
        int nx=tp->width();
        const int NS=rbins; //2048;
        float scale=float(nx/2)/(2048);

        const float v_scale=(1024/float(80*1875)); //Simulate an 80NM image over 1024 pixel
        float vxscaled=vx*v_scale;
        float vyscaled=vy*v_scale;
        vxscaled*=delta_t;
        vyscaled*=delta_t;
        cx+=vxscaled;
        cy+=vyscaled;

        if (cx<0)
            cx=nx+cx;
        if (cy<0)
            cy=nx+cy;
        if (cx>=nx)
            cx=cx-nx;
        if (cy>=nx)
            cy=cy-nx;

        float sa=angle; //sweep_angle-ptaz;

        for(int i=0; i<NS; ++i)
        {
            float accumulator=0;
            int acc_avg=0;

            //const float bw=1;//d2r(ui->beamWidth->value());
            //const float left_margin=bw; //0.001;
            //const float right_margin=bw+0.00000001;
            const float step=0.001;
            //for(float a=sa-left_margin; a<=sa+right_margin; a+=step)
            for(int k=0; k<1; ++k)//k=-1; k<=1; ++k)
            {
                float a=sa+step*k;

                float s=sinf(a);
                float c=cosf(a);

                float x=i*s;
                float y=i*c;

                x*=scale;
                y*=scale;

                x+=cx;
                y+=cy; //cy-y;

                //int xi=int(x) % nx;
                //int yi=int(y) % nx;
                if (x<0)
                    x=nx+x;
                if (y<0)
                    y=nx+y;
                if (x>=nx)
                    x=x-nx;
                if (y>=nx)
                    y=y-nx;

                float w=1;//(2-fabsf(sa-a))/2;
                w=1-abs(k)*0.5;
                int yi=irnd(y);
                int xi=irnd(x);
                //float a00=testPattern->scanLine(y+0.5)[(int)(x+0.5)];
                //float a01=testPattern->scanLine(y+0.5)[(int)(x+1)];                
                accumulator+=(tp->scanLine(yi)[(int)(xi)]*w);
                ++acc_avg;
            }
            //sweep_data[i]=tp->scanLine(y)[(int)(x)];
            uint8_t raw=accumulator/1;
            //raw=raw>>1;
            //raw+=128;
            sweep_data[i]=raw; //accumulator/1;//acc_avg;
        }
        return sweep_data;
    }
};

//ScSimulator::ScSimulator(QObject *parent) : QObject(parent), p_(*new Implementation)
ScSimulator::ScSimulator() : p_(*new Implementation)
{
    p_.tstImageCreate();
}

unsigned int ScSimulator::nextSweep(float delta_time, float angle, float vx, float vy, unsigned int rbins, float**data)
{
    if (delta_time==0)
        delta_time=0.10;
    const float* s=p_.inverseSC(delta_time, vx, vy, -angle, rbins);
    *data=const_cast<float*>(s);
    return rbins;
}

void ScSimulator::reset()
{
    //p_.cx=p_.testPattern->width()/2;
    //p_.cy=p_.testPattern->height()-1; //p_.cx;
    p_.reset();
}

void ScSimulator::setCenter(int x, int y)
{
    if (x==0)
        x=p_.testPattern->width()/2;
    if (y==0)
        y=p_.testPattern->width()-1;
    p_.cx=x;
    p_.cy=y;
}
