#include <stdio.h>
#include <stdlib.h>

#define MEM0_ADDR  (0x07000000)
#define BMP0_ADDR  (0x08000000)
#define DMA0_ADDR  (0x09000000)

#define R_WIDTH 100   //基準画像の幅
#define R_HEIGHT 50  //基準画像の高さ
#define I_WIDTH 128   //入力画像の幅
#define I_HEIGHT 128  //入力画像の高さ

#define WSEIDO 50
#define HSEIDO 20


struct DMAControlRegister{
  unsigned int source;
  unsigned int destination;
  unsigned int length;
  unsigned int command;
};


struct bitMapHeader{
  unsigned short bfType __attribute__((packed));
  unsigned int bfSize __attribute__((packed));
  unsigned short bfReserved1 __attribute__((packed));
  unsigned short bfReserved2 __attribute__((packed));
  unsigned int offBits __attribute__((packed));
  unsigned int biSize __attribute__((packed));
  unsigned int biWidth __attribute__((packed));
  unsigned int biHeight __attribute__((packed));
  unsigned short biPlanes __attribute__((packed));
  unsigned short biBitCount __attribute__((packed));
  unsigned int biCompression __attribute__((packed));
  unsigned int biSizeImage __attribute__((packed));
  unsigned int biXPelsPerMeter __attribute__((packed));
  unsigned int biYPelsPerMeter __attribute__((packed));
  unsigned int biClrUsed __attribute__((packed));
  unsigned int biClrImportant __attribute__((packed));
};


struct color{
  unsigned char B;
  unsigned char G;
  unsigned char R;
};


float sqrtf_mics(float x){
  float s,last;

  if(x <= 0.0)
      return 0.0;

  if(x > 1)
      s = x;
  else
      s = 1;

  do{
    last = s;
    s = (x / s + s) * 0.5;
  } while(s < last);

  return last;
}


float fabs_mics(float x){
  return (x > 0) ? x : -x;
}

int getKeta(float v){
  if(v >= 9)
    return 9;
  else if (v >= 8)
    return 8;
  else if (v >= 7)
    return 7;
  else if (v >= 6)
    return 6;
  else if (v >= 5)
    return 5;
  else if (v >= 4)
    return 4;
  else if (v >= 3)
    return 3;
  else if (v >= 2)
    return 2;
  else if (v >= 1)
    return 1;
  else
    return 0;
}


/*
void setTarget(unsigned char **target, unsigned char *input, int counter){
  int i;
  int height = counter / (I_WIDTH - R_WIDTH + 1);
  int width = counter % (I_WIDTH - R_WIDTH + 1);

  for(i = 0; i < R_HEIGHT; ++i)
    {
      target[i] = &input[(height + i) * I_WIDTH + width * 3];
    }
}
*/


void setTarget(unsigned char **target, unsigned char *input, int counter){
  int i;
  int height = counter / (I_WIDTH - R_WIDTH + 1);
  int width = counter % (I_WIDTH - R_WIDTH + 1);

  for(i = 0; i < R_HEIGHT; ++i)
    {
      target[i] = &input[(height + i) * I_WIDTH * 3 + width * 3];
    }
}


int main(int argc, char *argv[]){
  int i,j,k,point = -1,rect[4],testcounter,testheight,testwidth,testindex;
  int man, sen, hyaku, juu, iti, corr;
  float a,b,c,tmp,totalR,totalI,correlation = 0;
  unsigned char *input,*reference,*target[R_HEIGHT],*c1;
  struct bitMapHeader *iHeader,*rHeader;
  FILE *rfp1,*rfp2,*wfp;

  unsigned char *str, *mem0_addr;


  if(argc != 3)
    {
      fprintf(stderr, "usage: %s input_image reference_image\n", argv[0]);
      return 1;
    }

  if ((rfp1 = fopen(argv[1], "rb")) == NULL || (rfp2 = fopen(argv[2], "rb")) == NULL)
    {
      fprintf(stderr, "file open error!!\n");
      return 1;
    }

  //200000 -> 360000 -> 500000

  str = (unsigned char *)malloc(500000 + 54 + R_WIDTH * R_HEIGHT * 3);
  if(str == NULL){
    printf("メモリが確保できません\n");
    exit(1);
  }

  mem0_addr = str;

  c1 = mem0_addr;
  for(i = 0; i < I_WIDTH * I_HEIGHT * 3 + 54; ++i)
    {
      fread(c1++, 1, 1, rfp1);
    }

  c1 = (unsigned char*)(mem0_addr + 500000);
  for(i = 0; i < R_WIDTH * R_HEIGHT * 3 + 54; ++i)
    {
      fread(c1++, 1, 1, rfp2);
    }

  fclose(rfp2);
  fclose(rfp1);

  iHeader = (struct bitMapHeader*)mem0_addr;
  rHeader = (struct bitMapHeader*)(mem0_addr + 500000);

  input = (unsigned char*)(mem0_addr + 54);
  reference = (unsigned char*)(mem0_addr + 500000 + 54);

  if(!(iHeader->biWidth == I_WIDTH && iHeader->biHeight == I_HEIGHT && rHeader->biWidth == R_WIDTH && rHeader->biHeight == R_HEIGHT))
    {
      fprintf(stderr, "picture size is wrong!!\n");
      return 1;
    }


  c = totalR = 0;
  for(i = 0; i < R_HEIGHT * R_WIDTH * 3; i += R_WIDTH * 3 * HSEIDO)
    {
      for(j = 0; j < R_WIDTH * 3; j += 3 * WSEIDO)
	{
	  totalR += reference[i + j];
	  c += reference[i + j] * reference[i + j];
	}
    }
  c = c * R_WIDTH * R_HEIGHT - totalR * totalR;

  for(k = 0; k < (I_WIDTH - R_WIDTH + 1) * (I_HEIGHT - R_HEIGHT + 1); ++k)
    {
      a = b = 0;
      totalI = 0;


      //////////////setTarget(target, input, k);///////////////
      testheight = k / (I_WIDTH - R_WIDTH + 1);
      testwidth = k % (I_WIDTH - R_WIDTH + 1);

      for(i = 0; i < R_HEIGHT; ++i)
	{
	  target[i] = &input[(testheight + i) * I_WIDTH * 3 + testwidth * 3];
	}
      /////////////////////////////////////////////////////////


      for(i = 0; i < R_HEIGHT; i += 1 * HSEIDO)
	{
	  for(j = 0; j < R_WIDTH * 3; j += 3 * WSEIDO)
	    {
	      totalI += target[i][j];
	      a += target[i][j] * reference[i * R_WIDTH * 3 + j];
	      b += target[i][j] * target[i][j];
	    }
	}

      //printf("b = %lf\n",b);

      a = a * R_WIDTH * R_HEIGHT - totalI * totalR;
      b = b * R_WIDTH * R_HEIGHT - totalI * totalI;

      if(b * c <= 0)
	continue;

      tmp = fabs_mics(a / sqrtf_mics(b * c) * 10000);

      if(correlation < tmp)
	{
	  correlation = tmp;
	  point = k;
	}

    }

    rect[0] = (point / (I_WIDTH - R_WIDTH + 1)) * I_WIDTH + (point % (I_WIDTH - R_WIDTH + 1));
    rect[1] = rect[0] + I_WIDTH * R_HEIGHT + R_WIDTH;
    rect[2] = rect[0] + I_WIDTH * R_HEIGHT;
    rect[3] = rect[0] + R_WIDTH;


  c1 = input;
  for(i = 0; i < I_WIDTH * I_HEIGHT; ++i)
    {
      if(((rect[0] <= i && i <= rect[1]) && ((i % I_WIDTH == rect[0] % I_WIDTH) || (i % I_WIDTH == rect[1] % I_WIDTH))) || (rect[0] <= i && i <= rect[3]) || (rect[2] <= i && i <= rect[1]))
	{
	  *c1++ = 0;
	  *c1++ = 0;
	  *c1++ = 255;
	}
      else
	c1 += 3;
    }

  //---------------------------------------------------------------------------------

  if((wfp = fopen("result.bmp", "wb")) == NULL)
    {
      fprintf(stderr, "file open error!!\n");
      return 1;
    }

  fwrite(iHeader, sizeof(struct bitMapHeader), 1, wfp);//ヘッダの書き込み

  c1 = input;
  for(i = 0; i < I_WIDTH * I_HEIGHT * 3; ++i)
    {
      fwrite(c1++, 1, 1, wfp);
    }

  fclose(wfp);

  free(str);
  return 0;
}
