/***************************************************************************************
*	This software simulated for ARPEES, a routing protocol for Wireless Sensor Network *
*	Copyright (C) 2007 by Tran Quang Vinh (vinhtq@mail.hut.edu.vn)                     *
*	Multimedia Information NETwork Lab - MINET                                         *
*	Dept. Electronic Information Systems, Graduate School of Enginnering               *
*	Shibaura Institute of Technology, Japan.                                           *
*--------------------------------------------------------------------------------------*
*	File name: SensingCoverage.cpp									                   *
*	Implement the algorithm to find the redundance node in WSNs 				       *
***************************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <omnetpp.h>
#include "ArpeesDef.h"


//******************************************************************************************
void swapDouble ( double &x, double &y ) 
{ 
   double temp; 
   temp = x; 
   x = y; 
   y = temp; 
} 
//******************************************************************************************
void swapInt ( int &x, int &y ) 
{ 
   int temp; 
   temp = x; 
   x = y; 
   y = temp; 
} 
//******************************************************************************************
int circle_circle_intersection(double x0, double y0, double r0,
                               double x1, double y1, double r1,
                               double *xi, double *yi,
                               double *xi_prime, double *yi_prime)
{
    double a, dx, dy, d, h, rx, ry;
    double x2, y2;

    /* dx and dy are the vertical and horizontal distances between the circle centers */
    dx = x1 - x0;
    dy = y1 - y0;

    /* Determine the straight-line distance between the centers. */
    d = hypot(dx,dy); // Suggested by Keith Briggs
    //ev << "distance between two circles: d =  " << d << endl;
  
	/* Check for solvability. */
	if (d > (r0 + r1))
	{
		//ev << "no solution. circles do not intersect!" << endl;
		return 0;
	}
	if (d < fabs(r0 - r1))
	{
		//ev << "no solution. one circle is contained in the other!" << endl;
		return 0;
	}

	/* 'point 2' is the point where the line through the circle
	* intersection points crosses the line between the circle
	* centers.  
	*/

	/* Determine the distance from point 0 to point 2. */
	a = ((r0*r0) - (r1*r1) + (d*d)) / (2.0 * d) ;

	/* Determine the coordinates of point 2. */
	x2 = x0 + (dx * a/d);
	y2 = y0 + (dy * a/d);

	/* Determine the distance from point 2 to either of the intersection points */
	h = sqrt((r0*r0) - (a*a));

	/* Now determine the offsets of the intersection points from point 2 */
	rx = -dy * (h/d);
	ry = dx * (h/d);

	/* Determine the absolute intersection points. */
	*xi = x2 + rx;
	*xi_prime = x2 - rx;
	*yi = y2 + ry;
	*yi_prime = y2 - ry;

	return 1;
}

//****************************************************************************************

double get_counterclockwise_angle_in_radians(double ax, double ay, 
											 double bx, double by)
{
   double dotproduct, lengtha, lengthb, result;

   dotproduct = (ax * bx) + (ay * by);
   lengtha = sqrt(ax * ax + ay * ay);
   lengthb = sqrt(bx * bx + by * by);

   result = acos( dotproduct / (lengtha * lengthb) );
   //ev << "dotproduct = " << dotproduct << endl;
   //ev << "resutl = " << result << endl;	
   
   if (ay < 0)
	   result = 2*M_PI - result;
   return result;
}

//****************************************************************************************
int intersectionAngle2Circles(double x0, double y0, double r0,
						      double x1, double y1, double r1, 
						      double *secA1, double *secA2, double *secB1, double *secB2)
{
	double InterSectionP1_x, InterSectionP1_y, InterSectionP2_x, InterSectionP2_y;

	//ev << "x0=" << x0 << " y0=" << y0 << " R0=" << r0 << endl
	//	 << "x1=" << x1 << " y1=" << y1 << " R1=" << r1 << endl;  
  
	circle_circle_intersection(x0, y0, r0, x1, y1, r1,
                               &InterSectionP1_x, &InterSectionP1_y, 
							   &InterSectionP2_x, &InterSectionP2_y);

	//ev << "Intersection point 1: (" << InterSectionP1_x << "," << InterSectionP1_y << ")" << endl
	//     << "Intersection point 2: (" << InterSectionP2_x << "," << InterSectionP2_y << ")" << endl;
 
	double d = hypot(InterSectionP2_x - InterSectionP1_x,InterSectionP2_y - InterSectionP1_y);
	//ev << "distance between two intersection points: " << d << endl; 

	double angle1, angle2;
	double vecterP1_x, vecterP1_y,vecterP2_x,vecterP2_y;
	
	// translate the co-ordinate system
	vecterP1_x = InterSectionP1_x-x0;
	vecterP1_y = InterSectionP1_y-y0;
	vecterP2_x = InterSectionP2_x - x0;
	vecterP2_y = InterSectionP2_y - y0;
	
	//ev << "vecterP1: x = " << vecterP1_x << ", y = " << vecterP1_y << endl
	//	 << "vecterP2: x = " << vecterP2_x << ", y = " << vecterP2_y << endl;
	
	angle1 = get_counterclockwise_angle_in_radians(vecterP1_x, vecterP1_y, 1,0);
	angle2 = get_counterclockwise_angle_in_radians(vecterP2_x, vecterP2_y, 1,0);
	
	double temp = 0;
	if (angle1 > angle2)
	{
		swapDouble (angle1, angle2);
	}
	//ev << "angle1 = " << angle1 << " (radian)" << " = " << (angle1 * 180)/M_PI << " (degree)" << endl;
	//ev << "angle2 = " << angle2 << " (radian)" << " = " << (angle2 * 180)/M_PI << " (degree)" << endl;
	
	double critical_angle = 0;
	if (x1 < x0) // side angle II, III
	{
		critical_angle = (angle2 - angle1) * 180/M_PI;
		*secA1 = angle1;
		*secA2 = angle2;
		*secB1 = 0.0;
		*secB2 = 0.0;
				
		//ev << "critical angle: " << critical_angle << " degree" << endl;
		//ev << "[" << (int)((angle1 * 180)/M_PI) << ";" << (int)((angle2 * 180)/M_PI) << "]" << endl;
	}
	else
	{
		if (y1 < y0)// side angle IV
		{
			if (angle1 > M_PI)
			{
				critical_angle = (angle2 - angle1) * 180/M_PI;
				*secA1 = angle1;
				*secA2 = angle2;
				*secB1 = 0.0;
				*secB2 = 0.0;
				//ev << "critical angle: " << critical_angle << " degree" << endl;
				//ev << "[" << (int)((angle1 * 180)/M_PI) << ";" << (int)((angle2 * 180)/M_PI) << "]" << endl;
			}
			else
			{
				critical_angle = ((2*M_PI - angle2) + angle1) * 180/M_PI;
				*secA1 = angle2;
				*secA2 = 2*M_PI;
				*secB1 = 0.0;
				*secB2 = angle1;
				//ev << "critical angle: " << critical_angle << " degree" << endl;
				//ev << "[" << (int)((angle2 * 180)/M_PI) << ";360]" << "+[0;" << (int)((angle1 * 180)/M_PI) << "]" << endl;
			}
		}
		else // side angle I
		{
			if (angle1 < M_PI/6)
			{
				critical_angle = (angle2 - angle1) * 180/M_PI;
				*secA1 = angle1;
				*secA2 = angle2;
				*secB1 = 0.0;
				*secB2 = 0.0;
				//ev << "critical angle: " << critical_angle << " degree" << endl;
				//ev << "[" << (int)((angle1 * 180)/M_PI) << ";" << (int)((angle2 * 180)/M_PI) << "]" << endl;
			}
			else
			{
				critical_angle = ((2*M_PI - angle2) + angle1) * 180/M_PI;
				*secA1 = angle2;
				*secA2 = 2*M_PI;
				*secB1 = 0.0;
				*secB2 = angle1;
				//ev << "critical angle: " << critical_angle << " degree" << endl;
				//ev << "[" << (int)((angle2 * 180)/M_PI) << ";360]" << "+[0;" << (int)((angle1 * 180)/M_PI) << "]" << endl;
			}
		}
	}
	return 1;
}
//******************************************************************************
