#include "mm.h"

#define CHECK true
#define CHECK2 false

// matricna transformacija koordinat
void mm (xy_t x, xy_t y, xy_t *fx, xy_t *fy, m_t m[4])
{
#pragma HLS PIPELINE
#pragma HLS ARRAY_RESHAPE variable=m complete dim=1
	*fx = m[0]*x + m[1]*y;
	*fy = m[2]*x + m[3]*y;
}


void transform (dat_t p[size*size], dat_t q[size*size], m_t m[4])
{
#pragma HLS INTERFACE bram port=q
#pragma HLS INTERFACE bram port=p
#pragma HLS RESOURCE variable=q core=RAM_1P_BRAM
#pragma HLS RESOURCE variable=p core=RAM_1P_BRAM
#pragma HLS INTERFACE ap_ctrl_none port=return
	xy_t x, y, fx, fy;
	int i;
	int pi;

	loop: for (i = 0; i<size*size; i++)
	{
#pragma HLS PIPELINE
      x = i & 0x01f;
      y = i >> 5;

      if (CHECK) { // test index
    	  if (!(x>=0 && x<size && y>=0 && y<size)) {
    		  std::cout << "(x,y): " << x << "," << y << std::endl;
    	  }
      }

	  mm(x, y, &fx, &fy, m);

	  pi = ( ((int)fy) << 5) + (int)fx;

	  if (CHECK2) { // test transf. index
		  if (!(fx>=0 && fx<size && fy>=0 && fy<size)) {
			  std::cout << "(fx,fy): " << fx << "," << fy << std::endl;
		  }
		  if (!(pi>0 && pi<size*size)) {
			  std::cout << "qi: " << pi << std::endl;
		  }
	  }

	  if (pi>=0 && pi<size*size) q[i]= p[pi];
	  else q[i] = '2';

	}


}