/***************************************************************************
 *   Copyright (C) 2007 by Mauro,,,					   *
 *   mauro.r@ua.pt							   *
 *                                                                         *
 *   PhUA - Projecto Humanóide da Universidade de Aveiro 07/08		   *
 ***************************************************************************/
#ifndef HUMANMOV_CPP
#define HUMANMOV_CPP

#include "humanmov.h"
#include "visiondefs.h"
#include "mytrig.h"

void scan(signed char val[])
{
	int scanx, scany;

	scanx = (int)val[1];
	scany = (int)val[0];

	if(scanx >=0 && scanx < ang_max_pan && scany >=0){
		val[0] = 0;
		val[1] = scanx + 5;
		if(scany == 0)
			pp = 1;
		return;
	}
	if(scanx >= ang_min_pan && scanx < 0 && scany >= 0){
		val[0] = 0;
		val[1] = scanx + 5;
		return;
	}
	if(scanx <= ang_max_pan && scany <= 0){
		val[0] = (signed char)(scany - (5 * pp));
		if(scany <= ang_min_tilt)
			pp = -1;
		val[1] = (signed char)(scanx - 5);
		if(val[0] < ang_min_tilt)
			val[0] = ang_min_tilt;
		if(val[0] > ang_max_tilt)
			val[0] = ang_max_tilt;
		return;
	}
}

int RefCoordBallCM_2(signed char val[], double coord[], int Cxp, int Cyp)
{
	double qi0 = (double)val[1];
	double qi1 = -(double)val[0];
//	cout << "val[0]_tilt= " << qi0 << "(º)\t" << "val[1]_pan= " << qi1 << "(º)" << endl;
//	cout << "deltax= " << Cxp << "\t" << "deltay= " << Cyp << endl;
	// Para calcular a distância em metros na imagem é necessário multiplicar a distância em pixels (C#) pelo tamanho do pixel (5,6e-6) e pela razão entre a resolução máxima e a resolução de trabalho.
	double Cx = Cxp * 4 * 5.4 * pow(10,-6);
	double Cy = Cyp * 4 * 5.4 * pow(10,-6);
//	cout << "Cx= " << Cx << "\t" << "Cy= " << Cy << endl;
// %-- Expressao algebrica --------------------------------------------------------------
	double s1 = mysin(qi0);
	double c1 = mycos(qi0);
	double s2 = mysin(qi1);
	double c2 = mycos(qi1);
//	cout << "s1= " << s1 << "\t" << "c1= " << c1 << "\t" << "s2= " << s2 << "\t" << "c2= " << c2 << endl;
	double k1 = Lt + l3*c2 - (l2 + dfocal)*s2;
	double k2 = l2 + dfocal - Lt*s2;
	double k3 = (l3 + Lt*c2) * dfocal;
	double k4 = dfocal*s2 + Cy*c2;
//	cout << "k1= " << k1 << "\t" << "k2= " << k2 << "\t" << "k3= " << k3 << "\t" << "k4= " << k4 << endl;
	double xB = (k1*s1*Cx + k2*c1*Cy + k3*c1) / k4;
	double yB = (-k1*c1*Cx + k2*s1*Cy + k3*s1) / k4;
//	cout << "xB= " << xB << "(m)\n" << "yB= " << yB << "(m)" << endl;
	coord[0] = xB;
	coord[1] = yB;

	return 0; 
}

#endif