
#include "stdio.h"
#include "assert.h"
#include "math.h"
#include "Shared.h"

int  _Mode;

int  _BrightnessBias;
int  _ContrastScale;
int  _ColorScale;

int  _iLine;
int  _TotalLine;

double _dPPM_PAL; // bezüglich STM32-Takt

TConfig _Config;

TUSBItem_State _State;

TImg* ImgArr;

TBlit* Blit;

//################################################### TBlit
TBlit::TBlit(int nx, int ny, LPCSTR InfoTxt, int nxWnd, int nyWnd)
{
  if(nxWnd==0) nxWnd=nx;
  if(nyWnd==0) nyWnd=ny;
  // Init Window
  {
    WNDCLASS WndClass = { 0 };
    WndClass.style         = 0;
    WndClass.lpfnWndProc   = WndProc;
    WndClass.cbClsExtra    = 0;
    WndClass.cbWndExtra    = 0;
    WndClass.hInstance     = NULL;
    WndClass.hIcon         = NULL;
    WndClass.hCursor       = LoadCursor(0, IDC_ARROW);
    WndClass.hbrBackground = (HBRUSH)GetStockObject(GRAY_BRUSH);
    WndClass.lpszMenuName  = NULL;
    WndClass.lpszClassName = "sdfdsfsdfds";
    RegisterClass(&WndClass);
    _hWnd = CreateWindow("sdfdsfsdfds",
                         "",
                         WS_OVERLAPPEDWINDOW,
                         0,
                         0,
                         nxWnd,
                         nyWnd,
                         0,
                         NULL,
                         NULL,
                         this);
     RECT rc = { 0 };
     GetClientRect(_hWnd, &rc);
     SetWindowPos(_hWnd,
                  NULL,
                  0,
                  0,
                  2*nxWnd-(rc.right-rc.left),
                  2*nyWnd-(rc.bottom-rc.top),
                  SWP_NOZORDER | SWP_NOACTIVATE);
    char Tmp[0x100];
    sprintf(Tmp,
            "STM32 CVBS-Image-Grabber  [Mode %i : %i x %i; %s]",
            _Mode,
            nx,
            ny,
            InfoTxt);
    SetWindowText(_hWnd, Tmp);
    ShowWindow(_hWnd, SW_SHOW);
  }

  _nx = nx;
  _ny = ny;
  _MemDC = NULL;
  _DibHBitmap = NULL;
  _PnDC = ::GetDC(_hWnd);
  _DibPtr = NULL;
  _DibPitch = 0;
  int BPP = 32;
  _DibPitch = (_nx*BPP+31)/32;
  BITMAPINFO _BitmapInfo = { 0 };
  {
    BITMAPINFO* BMI = &_BitmapInfo;
    BMI->bmiHeader.biSize          = sizeof(BITMAPINFOHEADER);
    BMI->bmiHeader.biPlanes        = 1;
    BMI->bmiHeader.biBitCount      = BPP;
    BMI->bmiHeader.biCompression   = BI_RGB;
    BMI->bmiHeader.biSizeImage     = 4*_DibPitch*_ny;
    BMI->bmiHeader.biXPelsPerMeter = 0;
    BMI->bmiHeader.biYPelsPerMeter = 0;
    BMI->bmiHeader.biClrUsed       = 0;
    BMI->bmiHeader.biClrImportant  = 0;
    BMI->bmiHeader.biWidth  = _nx;
    BMI->bmiHeader.biHeight = _ny;
  }
  _MemDC = CreateCompatibleDC(0);
  _DibHBitmap = CreateDIBSection(_MemDC,
                                 &_BitmapInfo,
                                 DIB_RGB_COLORS,
                                 (void**)&_DibPtr,
                                 NULL,
                                 NULL);
  SelectObject(_MemDC, _DibHBitmap);
}

//################################################### ~TBlit
TBlit::~TBlit()
{
  // ...
}

//################################################### GetDC
HDC TBlit::GetDC()
{
  return _PnDC;
}

//################################################### DoBlit
void TBlit::DoBlit()
{
  RECT rc = { 0 };
  GetClientRect(_hWnd, &rc);
  int qx = _nx;
  int qy = _ny;
  int zx = rc.right;
  int zy = rc.bottom;
  if(qx>zx) qx=zx;
  if(qy>zy) qy=zy;
  StretchBlt(_PnDC, 
             0,
             0,
             zx,
             zy,
             _MemDC,
             0,
             0,
             qx,
             qy,
             SRCCOPY);
}

//################################################### GetPixel
DWORD TBlit::GetPixel(int x, int y)
{
  if(x>=0 && y>=0 && x<_nx && y<_ny)
  {
    return _DibPtr[(_ny-1-y)*_DibPitch+x];
  }
  return 0;
}

//################################################### SetPixel
void TBlit::SetPixel(int x, int y, DWORD RGB)
{
  if(x>=0 && y>=0 && x<_nx && y<_ny)
  {
    _DibPtr[(_ny-1-y)*_DibPitch+x] = RGB;
    RGB = ((RGB&0xff0000)<<4) |
          ((RGB&0x00ff00)<<2) |
          ((RGB&0x0000ff)<<0);
  }
}

//################################################### SaveImg
int TBlit::SaveImg(LPCSTR Mask)
{
  int DestPitch = (_nx*24+31)/32*4;
  int n = sizeof(BITMAPINFOHEADER)+DestPitch*_ny+3;
  BITMAPINFOHEADER* bih = (BITMAPINFOHEADER*)malloc(n);
  ZeroMemory(bih, sizeof(BITMAPINFOHEADER));
  bih->biSize          = sizeof(BITMAPINFOHEADER);
  bih->biWidth         = _nx;
  bih->biHeight        = _ny;
  bih->biPlanes        = 1;
  bih->biBitCount      = 24;
  bih->biCompression   = BI_RGB;
  bih->biSizeImage     = DestPitch*_ny; 
  bih->biXPelsPerMeter = 0;
  bih->biYPelsPerMeter = 0;
  bih->biClrUsed       = 0;
  bih->biClrImportant  = 0;
  DWORD* q = _DibPtr;
  BYTE*  z = (BYTE*)bih+sizeof(BITMAPINFOHEADER);
  for(int y=0; y<_ny; y++)
  {
    for(int x=0; x<_nx; x++)
    {
      *(DWORD*)(z+y*DestPitch+3*x) = q[y*_DibPitch+x];
    }
  }
  char FileName[MAX_PATH];
  static int iNum = 0;
  if(Mask==NULL) Mask="s:\\1\\%04i.bmp";
  sprintf(FileName, Mask, iNum++);
  int dHndl = _lcreat(FileName, 0);
  unsigned char rawData[14] = {
	0x42, 0x4D, 0x9E, 0x61, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x36, 0x00,
	0x00, 0x00
  };
  _lwrite(dHndl, (char*)rawData, sizeof(rawData));
  _lwrite(dHndl, (char*)bih, n);
  _lclose(dHndl);
  free(bih);
  return iNum;
}

//################################################### WndProc
LRESULT TBlit::WndProc(HWND   hWnd,
                       UINT   uMsg,
                       WPARAM wParam,
                       LPARAM lParam)
{
  switch(uMsg)
  {
    case WM_CREATE  : {
                        LPCREATESTRUCT lpcs = (LPCREATESTRUCT)lParam; 
                        SetWindowLong(hWnd, GWL_USERDATA, (LONG)lpcs->lpCreateParams);
                      }
                      break;
    case WM_CHAR    : // STRG-C -> Bild in die Zwischenablage kopieren
                      if(wParam==VK_CANCEL)
                      {
                        TBlit &o = *(TBlit*)GetWindowLong(hWnd, GWL_USERDATA);
                        if(&o!=NULL)
                        {
                          int DestPitch = (o._nx*24+31)/32*4;
                          HANDLE hGlobal = GlobalAlloc(GMEM_MOVEABLE | GMEM_DDESHARE, 
                                                       sizeof(BITMAPINFOHEADER)+DestPitch*o._ny+3);
                          BITMAPINFOHEADER* bih = (BITMAPINFOHEADER*)GlobalLock(hGlobal);
                          ZeroMemory(bih, sizeof(BITMAPINFOHEADER));
                          bih->biSize          = sizeof(BITMAPINFOHEADER);
                          bih->biWidth         = o._nx;
                          bih->biHeight        = o._ny;
                          bih->biPlanes        = 1;
                          bih->biBitCount      = 24;
                          bih->biCompression   = BI_RGB;
                          bih->biSizeImage     = DestPitch*o._ny; 
                          bih->biXPelsPerMeter = 0;
                          bih->biYPelsPerMeter = 0;
                          bih->biClrUsed       = 0;
                          bih->biClrImportant  = 0;
                          DWORD* q = o._DibPtr;
                          BYTE*  z = (BYTE*)bih+sizeof(BITMAPINFOHEADER);
                          for(int y=0; y<o._ny; y++)
                          {
                            for(int x=0; x<o._nx; x++)
                            {
                              *(DWORD*)(z+y*DestPitch+3*x) = q[y*o._DibPitch+x];
                            }
                          }
                          OpenClipboard(hWnd);
                          EmptyClipboard();
                          SetClipboardData(CF_DIB, hGlobal);
                          CloseClipboard();
                          printf("(Clipboard-Copy done)\n");
                        }
                        return 0;
                      }
                      // Terminate
                      if(wParam==VK_ESCAPE)
                      {
                        PostMessage(hWnd, WM_CLOSE, 0, 0); 
                        return 0;
                      }
                      break;
    case WM_DESTROY : PostQuitMessage(0);
                      break;
  }
  return DefWindowProc(hWnd, uMsg, wParam, lParam);
}

static double* SinArr16 = NULL;
static int*    iSinArr16 = NULL;

//################################################### sin16_Init
void sin16_Init()
{
  if(SinArr16==NULL)
  {
    SinArr16 = (double*)malloc(4096*sizeof(*SinArr16));
    iSinArr16 = (int*)malloc(4096*sizeof(*SinArr16));
    for(int i=0; i<4096; i++)
    {
      SinArr16[i] = sin(i*2*M_PI/4096);
      iSinArr16[i] = int(SinArr16[i]*65536);
    }
  }
}

//################################################### sin16
double sin16(uint16_t v)
{
  // return sin(v*2*M_PI/65536);
  return SinArr16[v>>4];
}

//################################################### cos16
double cos16(uint16_t v)
{
  // return cos(v*2*M_PI/65536);
  return SinArr16[uint16_t(v+0x4000)>>4];
}

//################################################### isin16
int isin16(uint16_t v)
{
  // return int(sin(v*2*M_PI/65536)*65536);
  return iSinArr16[v>>4];
}

//################################################### icos16
int icos16(uint16_t v)
{
  // return int(cos(v*2*M_PI/65536)*65536);
  return iSinArr16[uint16_t(v+0x4000)>>4];
}

//################################################### AddToImgArr
void AddToImgArr(TUSBItem_Line* USBItem, int iFrame)
{
  sin16_Init();

  // Bestimmung der Phase des Farbträgers bezogen auf den Samplezeitpunkt
  // des ersten Burst-ADC-Werts.
  // 
  int16_t phiPal_adc = 0;
  bool OddLine = false; // Zeile mit invertierter V-Komponente

  // ColorBurst
  {
    int16_t DbgPhi = 0;
    static int dAdcCycles;

    // Die Phase vom Burst alterniert Zeile für Zeile um +-45% der
    // Farbträgerphase.

    // Phase vom Burst bezogen auf ADC-Samplezeitpunkt vom Burst
    int16_t phiBurst_adc = 0;
    bool BurstOk = USBItem->BurstPhi!=0xffff;
    {
      uint16_t v = USBItem->BurstPhi;
      int c0 = v&0x1fff;
      int c1 = 8191;
      if((v&0x2000)!=0)
      {
        int Tmp = c0;  
        c0 = c1;
        c1 = Tmp;
      }
      if((v&0x4000)!=0)
      {
        c1 = -c1;
      }
      if((v&0x8000)!=0)
      {
        c0 = -c0;
      }
      phiBurst_adc = int16_t(65536*atan2(c0, c1)/2/M_PI);
    }
// int iFrame = _State.VSyncCnt&0xff;
// if((iFrame&7)==0 && (iLine-22==287)) printf("%6i %5i  %i\n", iFrame, int8_t(phiBurst_adc>>8), BurstOk);
// if((iFrame&7)==0 && iLine-22>=280) printf("%i ", (USBItem->Flags&16)!=0);

    // theoretische Phasenänderung vom Farbträger seit vorherigem Burst-Wert
    int16_t phiCarrier = 0;
    {
      static int32_t pha16 = 0;
      pha16 += int16_t(dAdcCycles/1e6/_fADCMHz*fPAL*(1-0e-6)*65536);
      phiCarrier = int16_t(pha16);
    }

    // Phasenänderung vom Burst seit letztem Burst
    // - idealerweise ein konstanter Wert der jede Zeile um +-45° alterniert
    int16_t dphiBurst_opt = phiBurst_adc-phiCarrier;

    // Korrekturwert durch Frequenzabweichung zwischen STM32 zum FBAS-Farbträger
    int16_t dphi_Osc;
    {
      static bool    Prev_BurstOk1 = false; 
      static bool    Prev_BurstOk2 = false; 
      static int16_t Prev_phi1 = 0; 
      static int16_t Prev_phi2 = 0; 
      static int32_t phaOfs = 0;
      static int32_t phaOfs2 = 0;
      int16_t dphi = dphiBurst_opt-Prev_phi2;
      if(BurstOk && Prev_BurstOk2 && Prev_BurstOk1)
      {
        phaOfs += (dphi*1024/2-phaOfs)/100;
      } 
      _dPPM_PAL = 1.0*phaOfs/1024/65536*15625/fPAL*1e6;
      // 2e9/65536/65536*15625/fPAL;
      // DbgPhi = dphi;
      // DbgPhi = phaOfs/256/1; // Dbg: 0 +- 45°
      phaOfs2 -= phaOfs;
      Prev_BurstOk2 = Prev_BurstOk1;
      Prev_BurstOk1 = BurstOk;
      Prev_phi2 = Prev_phi1;
      Prev_phi1 = dphiBurst_opt;
      dphi_Osc = phaOfs2>>10;
    }

    // Phasenänderung vom Burst seit letztem Burst
    // - ein konstanter Wert der jede Zeile um +-45° alterniert
    int16_t dphiBurst = dphiBurst_opt+dphi_Osc;

    // Farbträgerphase bezogen auf ADC-Samplezeitpunkt vom Burst
    {
      static int16_t dphiBurst_prev;
      int16_t d = (dphiBurst-dphiBurst_prev)/2;
      if(abs(d)>=64*256)
      {
        d = (65536+dphiBurst-dphiBurst_prev)/2;
      }
      OddLine = d>0;
      // DbgPhi = d; // Dbg: 0° +- 45° (alternierend)
      DbgPhi = dphiBurst-d; // Dbg: ein über Sekunden wandernder Wert (360°/s wäre auch ok)
      phiPal_adc = phiBurst_adc-d;
      dphiBurst_prev = dphiBurst;
    }           
    static int Prev_dCycle = 0;
    dAdcCycles = 64*_fADCMHz+_State.HSync_Raw+Prev_dCycle;
    Prev_dCycle = (USBItem->Flags&4)!=0 ? +1 : -1;

    static int f1;
    static int f2;
    if(f2<0) phiPal_adc+=int(fPAL/1e6/_fADCMHz*65536);
    f2 = f1; 
    f1 = (USBItem->Flags&4)!=0 ? +1 : -1;
    if(1==2)
    {
      int8_t phi = DbgPhi>>8;
      int x = _TotalLine%1600;
      static HDC PnDC = GetDC(0);
      BitBlt(PnDC, x, 0, 1, 256, NULL, 0, 0, WHITENESS);
      SetPixel(PnDC, x, 128, 0xc0c0c0);
      SetPixel(PnDC, x, 128-int8_t(phi), (_TotalLine&1) ? 0xff0000 : 0x0000ff);
    }
  }

  // ImgArr
  if(ImgArr==NULL)
  {
    ImgArr = (TImg*)malloc((ImgMsk+1)*sizeof(*ImgArr));
    FillMemory(ImgArr, (ImgMsk+1)*sizeof(*ImgArr), 0*2048/256);
  }

  if(_iLine>=0 && _iLine<320)
  {
    TUSBItem_LineEx* Img = ImgArr[iFrame&ImgMsk].Img+_iLine;
    Img->Odd = OddLine;
    Img->Adc2 = (USBItem->Flags&8)!=0;
    Img->BurstOk = USBItem->BurstPhi!=0xffff;
    Img->SubImageOfs = _State.SubImageOfs;
    if(!Img->BurstOk)
    {
      // if(iLine>5 && iLine<310)printf("%i ", iLine);
    }

    static int16_t Prev = 0;
    if(Img->BurstOk) Prev=phiPal_adc;
    if(!Img->BurstOk && 1==2)
    {
      Prev = Prev-64*256;
      phiPal_adc = Prev;
      Img->BurstOk = true;
    }
    Img->BurstPhi = phiPal_adc;
    Img->y = 2*(_iLine-22)+(iFrame&1);
    int Min = USBItem->MinMax&0x0f;
    int Max = (USBItem->MinMax>>4);
    // if(iLine==100) printf("%02x ", USBItem->MinMax);
    if(Max==0) Max=16;
    // if(iLine>20 && iLine<300) if(Min>5) printf("%i ", iLine);
    bool Blank = false;
    /*
    if(_iLine>30 && _iLine<32)
    {
      printf("%4i %5i %5i\n",  Img->SubImageOfs, Min, Max);
      if(Img->SubImageOfs==16)Blank=true;
      if(Img->SubImageOfs==18)Blank=true;
      if(Img->SubImageOfs==20)Blank=true;
      if(Img->SubImageOfs==22)Blank=true;
    } */

    if(_Config.dADC2!=0)
      {
        for(int x=0; x<_nSample; x++)
        {
          uint8_t v = USBItem->Image[x]^x;
          int v0 = (v<<4)&0xf0;
          v0 = v0*Max;
          v0 = v0+Min*256;
if(Blank)v0=0;
          Img->ADC1[x] = v0;
          int v1 = (v<<0)&0xf0;
          v1 = v1*Max;
          v1 = v1+Min*256;
if(Blank)v1=0;
          Img->ADC2[x] = v1;
        }
      }
    else
      {
        for(int x=0; x<_nSample; x++)
        {
          int v = USBItem->Image[x]^x;
          v = v*Max;
          v = v+Min*256;
if(Blank)v=0;
          Img->ADC1[x] = v;
        }
      }
  }
}

//################################################### YUV_to_RGB
DWORD YUV_to_RGB(double Y, double U, double V)
{
  // " Reduktion" rückgängig machen
  //   https://de.wikipedia.org/wiki/YUV-Farbmodell:
  //   U = 0,493 · (B - Y)
  //   V = 0,877 · (R - Y)
  double CB = U/0.493;
  double CR = V/0.877;

  // Farbeinstellung
  {
    CB = CB*_ColorScale/100;
    CR = CR*_ColorScale/100;
  }

  Y += _BrightnessBias*1000*4096/3300/100; // bei 100% wird ein Black-Wert von 1000mV über den Blank-Wert angesetzt
  double R = Y-CR;
  double B = Y-CB;
  double G = (Y*1.0 - B*0.114 - R*0.299)/0.587;
  // Kontrasteinstellung
  {
    R *= _ContrastScale/100.0/(4096/256);
    G *= _ContrastScale/100.0/(4096/256);
    B *= _ContrastScale/100.0/(4096/256);
  }
  int r = int(R);
  int g = int(G);
  int b = int(B);
  if(r<  0) r=  0;
  if(b<  0) b=  0;
  if(g<  0) g=  0;
  if(r>255) r=255;
  if(b>255) b=255;
  if(g>255) g=255;
  DWORD rgb = (b<<0)|(g<<8)|(r<<16);
  return rgb; 
}

//################################################### InitLine
void InitLine(TUSBItem_LineEx* LineRef,
              TUSBItem_LineEx* LineCur,
              int dADCRef,
              int dADCCur)
{
  int p0 = LineRef->SubImageOfs-dADCRef;
  int p1 = LineCur->SubImageOfs-dADCCur;
  // assert(p0>=0);
  // assert(p1>=0);
  int dx = p1-p0;
  int dSample = 0;
  if(dx>=+_nSub/2) dSample=-1;
  if(dx<=-_nSub/2) dSample=+1;
  LineCur->xSub = p1 + dSample*_nSub;
  uint16_t dp = uint16_t(LineCur->xSub/1e6/_fADCMHz*fPAL*(1+_dPPM_PAL*1e-6)*65536);
  LineCur->Samples = (dADCCur!=0 ? LineCur->ADC2 : LineCur->ADC1)+dSample;
  LineCur->Phi = LineCur->BurstPhi+dp;
}

//################################################### LineToUV
void LineToUV(TUSBItem_LineEx* Line0,
              TUSBItem_LineEx* Line1,
              TUSBItem_LineEx* Line2,
              TUSBItem_LineEx* Line3,
              uint16_t Phi0,
              uint16_t iSample,
              double*  ResU,
              double*  ResV)
{
  // - Line0 und Line2
  //   - gleiche Pixelposition (eigentlich reicht die gleiche Luminanz)
  //   - Farbphasen um etwa 180° versetzt
  // - Line1 und Line3
  //   - gleiche Pixelposition (eigentlich reicht die gleiche Luminanz)
  //   - Farbphasen um etwa 180° versetzt
  // - Line0 und Line1
  //   - um 2 Pixel verschoben, leider nötig um die 90° zu erhalten
  //   - Farbphasen um etwa 90° versetzt
  // - Line0, Line1, Line2, Line3
  //    - alle Zeilen mit entweder invertierte oder nicht invertierte
  //      V-Komponente (Odd oder !Odd)
  if(Line0->BurstOk)
  {
    // idealerweise sollten die Meßwerte alle um 90° zueinander
    // verschoben sein. Die Phasen weichen aber um +-45° ab (Granulatät, kein
    // Meßfehler). Die tatsächliche Phase ist aber bekannt:
    uint16_t p0 = Phi0+Line0->Phi;
    uint16_t p1 = Phi0+Line1->Phi;
    uint16_t p2 = Phi0+Line2->Phi;
    uint16_t p3 = Phi0+Line3->Phi;
    // Abweichung rausrechnen:
    // (Das Rausrechnen geht zwar perfekt (keine Näherung), Meßfehler
    // werden dabei aber unvermeidbar (Mathematik) verstärkt.)
    uint16_t pp0 = (p0+p2)/2;
    uint16_t pp1 = (p1+p3)/2;
    double k0 = 2*sin16(pp1-pp0);
    double k1 = sin16((p2-p0)/2);
    double k2 = sin16((p3-p1)/2);
    // bei zusätzlichen Meßfehlern kann die Abweichung zu groß (um 90°)
    // werden und die Berechnung wird zu ungenau -> Farbe nur bei
    // ausreichenden Werten berechnen
    double gg = 0.1;
    if(fabs(k0)>gg && fabs(k1)>gg && fabs(k2)>gg)
    {
      // Ok, genau genug
      double vv0 = (Line0->Samples[iSample]-Line2->Samples[iSample])/k0/k1;
      double vv1 = (Line1->Samples[iSample]-Line3->Samples[iSample])/k0/k2;
      *ResU = vv1*sin16(pp0) - vv0*sin16(pp1);
      *ResV = vv1*cos16(pp0) - vv0*cos16(pp1);
      if(Line0->Odd) *ResV=-*ResV;
    }
  }
}

//################################################### LineToY
void LineToY(TUSBItem_LineEx* Line0,
             TUSBItem_LineEx* Line1,
             uint16_t Phi0,
             uint16_t iSample,
             double   U,
             double   V,
             double*  ResY)
{
  // Line1 darf NULL sein
  double Y = 0;
  for(int i=0; i<2; i++)
  {
    uint16_t p = Phi0+Line0->Phi;
    if(Line0->Odd) p=0x8000-p; // quasi V=-V
    Y += Line0->Samples[iSample] - ( + U*sin16(p) + V*cos16(p) );
    if(Line1!=NULL) Line0=Line1;
  }
  *ResY = Y/2;
}
