#define PIXEL_ROW 120
#define PIXEL_COL 160
#define HEADER_LENGTH 54
#define BMP_FIGURE_SIZE (PIXEL_ROW * PIXEL_COL * 3 + HEADER_LENGTH)

void opticalflow(unsigned int const img1[PIXEL_ROW * PIXEL_COL], unsigned int const img2[PIXEL_ROW * PIXEL_COL], int Ix[PIXEL_ROW * PIXEL_COL], int Iy[PIXEL_ROW * PIXEL_COL], int It[PIXEL_ROW * PIXEL_COL], int w, int Vx[PIXEL_ROW * PIXEL_COL], int Vy[PIXEL_ROW * PIXEL_COL])
{
    int i,j;

    // initialize
    for( i=0;i<PIXEL_ROW;i++)
    {
        for(j=0;j<PIXEL_COL;j++)
        {
            Ix[i*PIXEL_COL + j] = 0;
            Iy[i*PIXEL_COL + j] = 0;
            It[i*PIXEL_COL + j] = 0;
            Vx[i*PIXEL_COL + j] = 0;
            Vy[i*PIXEL_COL + j] = 0;
        }
    }

    // get Ix,Iy,It
    for( i=1;i<PIXEL_ROW-1;i++)
    {
        for(j=1;j<PIXEL_COL-1;j++)
        {
            Ix[i*PIXEL_COL + j] = img1[i*PIXEL_COL+j+1] - img1[i*PIXEL_COL+j-1];
            Iy[i*PIXEL_COL + j] = img1[(i+1)*PIXEL_COL+j] - img1[(i-1)*PIXEL_COL+j];
            It[i*PIXEL_COL + j] = img2[i*PIXEL_COL+j] - img1[i*PIXEL_COL+j];
        }
    }

    // get u,v for every pixel exclude the edge
    w = ((w + 1)/2 );
    
    int m=0,n=0;
    int A11=0,A12=0,A21=0,A22=0;
    int A11_inv=0,A12_inv=0,A21_inv=0,A22_inv=0;
    int b1=0,b2=0;
    for( i=w;i<PIXEL_ROW-w-1;i++)
    {
        for(j=w;j<PIXEL_COL-w-1;j++)
        {
            // in the window
            A11=0,A12=0,A21=0,A22=0;
	    b1=0,b2=0;

            for(m=-w;m<=w;m++)
            {
                for(n=-w;n<=w;n++)
                {
                    A11 = A11+(Ix[(i+m)*PIXEL_COL+j+n]*Ix[(i+m)*PIXEL_COL+j+n]);
                    A12 = A12+(Ix[(i+m)*PIXEL_COL+j+n]*Iy[(i+m)*PIXEL_COL+j+n]);
                    A21 = A21+(Iy[(i+m)*PIXEL_COL+j+n]*Ix[(i+m)*PIXEL_COL+j+n]);
                    A22 = A22+(Iy[(i+m)*PIXEL_COL+j+n]*Iy[(i+m)*PIXEL_COL+j+n]);
                    b1 = b1+(Ix[(i+m)*PIXEL_COL+j+n]*It[(i+m)*PIXEL_COL+j+n]);
                    b2 = b2+(Iy[(i+m)*PIXEL_COL+j+n]*It[(i+m)*PIXEL_COL+j+n]);
			
                }   
            }

	
	//printf("%d, ",A12);
		

            int dn=(A11*A22-A12*A21)/1000;
	    int u1,v1;
            if (0 != dn)
            {
                A11_inv = (A22*1000)/(dn);
                A12_inv = (-A12*1000)/(dn);
                A21_inv = (-A21*1000)/(dn);
                A22_inv = (A11*1000)/(dn);
                
                b1 = -b1;
                b2 = -b2;
                
		u1 = A11_inv*b1+A12_inv*b2;
		v1 = A21_inv*b1+A22_inv*b2;

                Vx[i*PIXEL_COL + j] = u1;
                Vy[i*PIXEL_COL + j] = v1;
            }
	    else
	    {
		Vx[i*PIXEL_COL + j] = 0;
                Vy[i*PIXEL_COL + j] = 0;
	    }
//	    printf("%d, ",Vx[i*PIXEL_COL + j]);
        }
//	 printf("\n\n");
    }

}