#include <math.h>
#include "Warp.h"

//*******************************************
// static void initQuadri(QUADRI *q)        *
// Inicializa o padrao de coordenadas a ser *
// aplicado aos pontos mapeados e chama o   *
// calcula_matriz.                          *
//*******************************************
static void initQuadri(QUADRI *q)
{
    padrao.u0 = 0;
    padrao.v0 = 0;
    padrao.tam_u = 480;
    padrao.tam_v = 400;

    calcula_matriz(q);
}

//**********************************************
// static void calcula_matriz(QUADRI *pol)     *
// Calcula a matriz de transformacao de acordo *
// com os pontos que descrevem o poligono em   *
// perspectiva.                                *
//**********************************************
static void calcula_matriz(QUADRI *pol)
{
  double dx1,dx2,dx3,dy1,dy2,dy3,n_zero;
  double  X[5],  Y[5];
  int transf_afim;

  X[0] = DB pol->x0;
  Y[0] = DB pol->y0;
  X[1] = DB pol->x1;
  Y[1] = DB pol->y1;
  X[2] = DB pol->x2;
  Y[2] = DB pol->y2;
  X[3] = DB pol->x3;
  Y[3] = DB pol->y3;

  dx1 = X[1] - X[2];
  dy1 = Y[1] - Y[2];
  dx2 = X[3] - X[2];
  dy2 = Y[3] - Y[2];
  dx3 = X[0] - X[1] + X[2] - X[3]; //se 0, as retas sao paralelas
  dy3 = Y[0] - Y[1] + Y[2] - Y[3]; //idem

  if( dx3==0.0 && dy3==0.0 )     /* transformacao afim */
  {
    transf_afim = 1;
    A[1][1] = X[1] - X[0];
    A[2][1] = X[2] - X[1];
    A[3][1] = X[0];
    A[1][2] = Y[1] - Y[0];
    A[2][2] = Y[2] - Y[1];
    A[3][2] = Y[0];
    A[1][3] = 0.0;
    A[2][3] = 0.0;
  }
  else                         /* transformacao perspectiva */
  {
    transf_afim = 0;
    n_zero = dx1 * dy2 - dy1 * dx2;
    A[1][3] = (dx3 * dy2 - dy3 * dx2) / n_zero;
    A[2][3] = (dx1 * dy3 - dy1 * dx3) / n_zero;
    A[1][1] = X[1] - X[0] + A[1][3] * X[1];
    A[2][1] = X[3] - X[0] + A[2][3] * X[3];
    A[3][1] = X[0];
    A[1][2] = Y[1] - Y[0] + A[1][3] * Y[1];
    A[2][2] = Y[3] - Y[0] + A[2][3] * Y[3];
    A[3][2] = Y[0];
  }
  A[3][3] = 1.0;
  inverte(transf_afim);
}

//******************************************
// static void inverte(int eh_afim)        *
// Modifica os valores da matriz calculada *
// por calcula_matriz de acordo com o tipo *
// de transformacao em questao (afim ou    *
// perspectiva).                           *
//******************************************
static void inverte(int eh_afim)
{
  double det,n_zero;

  if (eh_afim)
  {
    n_zero = A[1][1] * A[2][2] - A[2][1] * A[1][2];
    det     = 1 / n_zero;
    M[1][1] = det * A[2][2];
    M[1][2] = det * ( - A[1][2]);
    M[1][3] = 0;
    M[2][1] = det * ( - A[2][1]);
    M[2][2] = det * A[1][1];
    M[2][3] = 0;
    M[3][1] = det * (A[2][1] * A[3][2] - A[3][1] * A[2][2]);
    M[3][2] = det * (A[3][1] * A[1][2] - A[1][1] * A[3][2]);
    M[3][3] = det * (A[1][1] * A[2][2] - A[2][1] * A[1][2]);
  }
  else  /* eh perspectiva */
  {
    M[1][1] = A[2][2] * A[3][3] - A[2][3] * A[3][2];
    M[1][2] = A[1][3] * A[3][2] - A[1][2] * A[3][3];
    M[1][3] = A[1][2] * A[2][3] - A[1][3] * A[2][2];
    M[2][1] = A[2][3] * A[3][1] - A[2][1] * A[3][3];
    M[2][2] = A[1][1] * A[3][3] - A[1][3] * A[3][1];
    M[2][3] = A[1][3] * A[2][1] - A[1][1] * A[2][3];
    M[3][1] = A[2][1] * A[3][2] - A[2][2] * A[3][1];
    M[3][2] = A[1][2] * A[3][1] - A[1][1] * A[3][2];
    M[3][3] = A[1][1] * A[2][2] - A[1][2] * A[2][1];
  }
}

//**************************************************************
// static void mapeia(int x, int y, int *u, int*v)             *
// Atribui para as variaveis u e v as coordenadas horizontas   *
// e vertical, respectivamente, correspondentes a posicao      *
// no plano do ponto (x,y) da perspectiva.                     *
//**************************************************************
static void mapeia (int x,int y,int *u,int *v)
{

   double uu, vv, z, xx, yy;

   xx = (double)(x);
   yy = DB(y);

   z = M[1][3] * xx + M[2][3] * yy + M[3][3];
   if( z==0.0 )  z = 1.0E-500;
   uu = ( M[1][1] * xx + M[2][1] * yy + M[3][1] ) / z;
   vv = ( M[1][2] * xx + M[2][2] * yy + M[3][2] ) / z;
/* Neste ponto uu e vv tem valores entre [0..1] */
   *u = ( int )(( uu * padrao.tam_u) + padrao.u0);
   *v = ( int )(( vv * padrao.tam_v) + padrao.v0);
}