/***************************************************************************************
*	This software simulated for ARPEEC, 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: SensorNode.cpp										                   *
*	Implement the node in the network									               *
***************************************************************************************/

#include <stdio.h>
#include <string.h>

#include <omnetpp.h>
#include "SensorNode.h"
#include "BaseStation.h"
#include "MessageType_m.h"
#include "SensingCoverage.h"

Define_Module(SensorNode);

/**************************************************************************************/
void SensorNode::initialize()
{
    cModule *parent = parentModule();

    this->initStats();
    this->myId = par("id");
    this->xpos = par("xpos");
    this->ypos = par("ypos");
    this->nrGates = (int) parent->par("numNodes") + 3;
    this->energy = (double) par("energy");
    this->myCluster = 0;
    this->nrNodes = (int) parent->par("numNodes");
    this->nrFrames = (int) parent->par("frames");
    this->nrRounds = (int) parent->par("rounds");
	this->trRange = (int) parent->par("trRange");
	this->ssRange = (int) parent->par("ssRange");
	this->kRequire = (int) parent->par("kRequire");
	this->showHideRange = (int) parent->par("showHideRange");


    this->myData = 0;
    this->curFrame = 0;
    this->wasHead = 0;
    this->lastClusterRound = 0;
	this->hopCount = 0;
	this->neighber = 0;

    // set network default
    this->myStatus = STATUS_LIVE;
	//this->setDisplayString("p=$xpos,$ypos;i=misc/node_vs"); // grey color is default
    this->setDisplayString("p=$xpos,$ypos;i=block/wrx_vs");
	
	if (showHideRange == ON)
	{
		this->setDisplayString("p=$xpos,$ypos;i=misc/node_vs;r=$ssRange,,blue,1");
	}
	else if (showHideRange == OFF)
	{
		this->setDisplayString("p=$xpos,$ypos;i=misc/node_vs");
	}
	
    ev <<"node "<<this->myId - 2 <<" energy: "<<energy<<" status: "<<this->myStatus<<"\n";
    {
        cMessage *cmsg = new cMessage();
        cmsg->setKind(SMSG_INIT);                      //17, init updates
        scheduleAt(simTime() + INIT_WAIT, cmsg);
    }
}
/**************************************************************************************/
// building the initial network simulation
void SensorNode::initNodes()
{
    int i;
    cModule *parent = parentModule();
    cModule *mod;
    int numNodes;
    int ritems;

    numNodes = (int) parent->par("numNodes");
    ev << "numNodes is: " << numNodes << " energy: " << energy << "\n";
	
	for (i = 1; i <= simulation.lastModuleIndex(); i++)
    {
        int x, y, id;

		//scan the simulation module vector
        mod = (cModule *) simulation.module(i);

        // check for nodes in transmission range
        if ((strcmp(mod->name(), "node") == 0) || (strcmp(mod->name(), "bs") == 0))
        {
            if (strcmp(mod->name(), "node") == 0)
            {
                id = ((SensorNode *) mod)->myId;
                x = ((SensorNode *) mod)->xpos;
                y = ((SensorNode *) mod)->ypos;
                nodePtr[id] = ((SensorNode *) mod);
            }
            if (strcmp(mod->name(), "bs") == 0)
            {
                id = ((BaseStation *) mod)->myId;
                this->bsId = id;
                x = ((BaseStation *) mod)->xpos;
                y = ((BaseStation *) mod)->ypos;
                
				this->bsDist = (x - this->xpos) * (x - this->xpos) +
                               (y - this->ypos) * (y - this->ypos);
            }
            if (id != this->myId && id == this->bsId)
            {
                cGate *g;
                char gName[32];
                int items;

                if ( id== this->bsId)
				{
                    items = this->gatev.items();
                    ritems = mod->gatev.items();

                    // make new gate here
                    sprintf(gName, "O_%d", id);
                    g = new cGate(gName, 'O');
                    this->gatev.addAt(items, g); // position, element
                    g->setOwnerModule((cModule *) this, items);
					//hide the arrow from node to BS
					g->displayString().setTagArg("o",1,"0");

                    // make new gate at other side
                    sprintf(gName, "I_%d", this->myId);
                    g = new cGate(gName, 'I');
                    mod->gatev.addAt(ritems, g); // position, element
                    g->setOwnerModule((cModule *) mod, ritems);

                    //CHANNEL
                    cLinkType *wless = findLink("wless");
                    connect((cModule*)this,items,(cLinkType*)wless,(cModule*)mod,ritems);
                }
            }
        }
    }
    // send energy status to BS
    ev << "call send send status to BS function \n";
    send2BS(this->myId, this->bsId, this->energy, this->myStatus, 
		    this->myCluster, this->xpos, this->ypos);
}
/**************************************************************************************/
int SensorNode::computeNeighbor()
{
	int count = 0;
	double	dis = 0;
	for (int n = 2; n < this->nrNodes + 2; n++)
    {
		if (nodePtr[n]->energy < 0)
		{
			nodePtr[n]->displayString().setTagArg("i",1,"black");
		}
		if (this->myId != nodePtr[n]->myId && nodePtr[n]->energy > 0)
			//&& this->myHead == nodePtr[n]->myHead // stay on the same cluster
		{
			// compute distance to other nodes
			dis =  ((nodePtr[n]->xpos - this->xpos)*(nodePtr[n]->xpos - this->xpos))
				  +((nodePtr[n]->ypos - this->ypos)*(nodePtr[n]->ypos - this->ypos));
			
			if ((int)sqrt(dis) < this->ssRange)
			{
				//ev << "distance from " << this->myId << " to it's neighbor " << nodePtr[n]->myId 
				//   << " is: " << (int)sqrt(dis) << endl;
				count++;
			}
		}
	}
	return count;
}

/**************************************************************************************/
int SensorNode::checkRedundance()
{
	
	int n = 0;
	double	dis = 0;
	int i = 0, j = 0;
	int numberNeighbors = 0;
	double sectionA1, sectionA2, sectionB1, sectionB2;
	double unionOperator = 0.0;
	
	struct sectionAngle
	{
		double begin;
		double end;
		int status;
	} secAngle[MAXNEIGHBORS];
	
	for (i = 0; i < MAXNEIGHBORS; i++)
	{
		secAngle[i].begin = 0.0;
		secAngle[i].end = 0.0;
		secAngle[i].status = 0;
	}
	// compute distance to other nodes
    i = 0; j = 0;
	for (n = 2; n < this->nrNodes + 2; n++)
    {
		if (this->myId != nodePtr[n]->myId && nodePtr[n]->myAction == 0 && nodePtr[n]->energy > 0.0)
			//&& this->myHead == nodePtr[n]->myHead
		{
			// compute distance to other nodes
			dis =  ((nodePtr[n]->xpos - this->xpos)*(nodePtr[n]->xpos - this->xpos))
				  +((nodePtr[n]->ypos - this->ypos)*(nodePtr[n]->ypos - this->ypos));
			
			if ((int)sqrt(dis) < this->ssRange)
			{
				//ev << "distance from " << this->myId << " to it's neighbor " << nodePtr[n]->myId 
				//   << " is: " << (int)sqrt(dis) << endl;

				intersectionAngle2Circles(this->xpos, this->ypos, ssRange, 
				                          nodePtr[n]->xpos, nodePtr[n]->ypos, ssRange,
										  &sectionA1, &sectionA2, &sectionB1, &sectionB2);
				
				// stored the section on the two-dimention array
				if (sectionB1 == 0.0 && sectionB2 == 0.0)
				{
					secAngle[i].begin = sectionA1;
					secAngle[i].end = sectionA2;
					//ev << "{" << section[i][0] << ";" << section[i][1] << "}" << endl;
				}
				else
				{
					secAngle[i].begin = sectionA1;
					secAngle[i].end = sectionA2;
					//ev << "{" << section[i][0] << ";" << section[i][1] << "}" << endl;
					i++;
					secAngle[i].begin = sectionB1;
					secAngle[i].end = sectionB2;
					//ev << "{" << section[i][0] << ";" << section[i][1] << "}" << endl;
				}
				
				i++;
				numberNeighbors++;
			}
			else
			{
				//ev << "node " << nodePtr[n]->myId << " out of sensing range!" << endl;
			}
		}
		else
		{
			//ev << "this node is not in my cluster" << endl;
		}
	}
	
	int totalSection = i;
	ev << "total neighbors: " << numberNeighbors << endl;

	// sort by increasing order of the first element of the two-dimention array
	for(i = 0; i < totalSection - 1; i++)
	{
		for(j = 0; j < totalSection - i - 1; j++)
		{
			// swap the elements depending on the first row if the next value is smaller
			if(secAngle[j+1].begin < secAngle[j].begin)  
			{
				swapDouble( secAngle[j+1].end, secAngle[j].end);
				swapDouble( secAngle[j+1].begin, secAngle[j].begin);
			}
			// else if the two elements are equal, sort them depending on the second row
			else if(secAngle[j+1].begin == secAngle[j].begin) 
			{
				if(secAngle[j+1].begin < secAngle[j].begin)
				{
					swapDouble( secAngle[j+1].end, secAngle[j].end);
					swapDouble( secAngle[j+1].begin, secAngle[j].begin);
				}
			}
		}
	}
	/*
	ev << "intersection angles:" << endl;
	for (i = 0; i < totalSection; i++)
	{
		ev << "{" << secAngle[i].begin << ";" << secAngle[i].end << "}" << endl;
	}
	*/

	//ev << "compute the union \n";  
	int kReturn = 0;
	int kCoverage = 0;
	int k = 0;
	
	for (k = 1; k <= this->kRequire; k++)
	{
		int control = 0;
		int i = 0,j = 0;
		int rec = 0;
		double Uni = 0;
		double max = 0.0;

		if (secAngle[k-1].begin != 0.0 && secAngle[k-1].status == 0)
		{
			//ev << "do not cover point 0" << endl;
			break;
		}
		else
		{
			for (j = 0; j < totalSection ; j++)	
			{
				for (i = control; i < totalSection ; i++)
				{
					if (secAngle[i].begin <= Uni)
					{
						if (secAngle[i].status == 0)
						{
							if (secAngle[i].end > max)
							{
								max = secAngle[i].end;
							}
						}
						control++;
						rec++;
					}
				}
				if (rec != 0)
				{
					//ev << "control = " << control << endl;
					
					Uni = max; // update Union
					max = 0.0; // reset max
					rec = 0;
					
					// update status
					for (int n = 0; n < totalSection; n++)
					{
						if (secAngle[n].end == Uni && secAngle[n].status == 0) 
						{
							secAngle[n].status = 1;
							//ev << n << " was be set status = 1" << endl;
							break;
						}
					}

					// check if done
					//ev << "Uni = " << Uni << endl;
					if (Uni >= 6.28 )
					{
						kCoverage++;
						//ev << "OK! k = " << k << endl;
						break;
					}
				}
				else
				{
					//ev << "break! No intersection" << endl;
					break;
				}
			}
		}
	}

	ev << "k-coverage = " << kCoverage << ", kRequire = " << kRequire << endl;
	if (kCoverage == kRequire)
		kReturn = 1;

	// return the result
	return kReturn;
}

/**************************************************************************************/
void SensorNode::handleMessage(cMessage * msg)
{
	if (!msg->isSelfMessage())
    {
        
		// free node send its status to bs, this is for simulation only!
		if (((ClusterMessage *) msg)->getProto() == CL_TO_FREE_NODE)
		{
			this->displayString().setTagArg("i",1, "grey");			// reset to default color
			this->setDisplayString("p=$xpos,$ypos;i=status/asleep;r=$ssRange,,blue,1");
			//this->displayString().removeTag("r");					// reset radio off
			//this->displayString().setTagArg("i",1,"green");		// set color to violet
			if (showHideRange == ON)
			{
				//this->setDisplayString("p=$xpos,$ypos;i=misc/node_vs;r=$ssRange,,blue,1");
				this->setDisplayString("p=$xpos,$ypos;i=status/asleep;r=$ssRange,,blue,1");
				this->displayString().setTagArg("i",1, "green");
			}
			else if (showHideRange == OFF)
			{
				this->displayString().setTagArg("i",1, "green");
			}

			// reduce energy for packet reception of header
            energyReceive(SIGNAL);
			
			ev << "node " << this->myId << " is free node, do nothing!" << endl;
			cMessage *cmsg = new cMessage();
            cmsg->setKind(SMSG_STATUS2BS);
            scheduleAt(simTime(), cmsg);
		}
		
		// redundance node send its action status to bs, this is for simulation only!
		if (((ClusterMessage *) msg)->getProto() == CL_TO_REDUN_NODE)
		{
			this->setDisplayString("p=$xpos,$ypos;i=misc/square_vs");
			//this->setDisplayString("p=$xpos,$ypos;i=misc/node_vs");
			this->displayString().setTagArg("i",1, "brown");			// reset to default color
			this->displayString().removeTag("r");					// reset radio off
			
			// reduce energy for packet reception of header
            energyReceive(SIGNAL);
			
			ev << "node " << this->myId << " is redundance node, enter sleep mode!" << endl;
			cMessage *cmsg = new cMessage();
            cmsg->setKind(SMSG_STATUS2BS);
            scheduleAt(simTime(), cmsg);
		}

		// BS announces node with this message by broadcasting
        if (((ClusterMessage *) msg)->getProto() == CL_TOHEAD)
        {
			int head;           // cluster head of the cluster
			
			if (showHideRange == ON)
			{
				this->setDisplayString("p=$xpos,$ypos;i=block/wrx_vs;r=$ssRange,,blue,1");
				this->displayString().setTagArg("i",1, "green");
			}
			else if (showHideRange == OFF)
			{
				//this->displayString().setTagArg("i",1, "green");
				this->displayString().removeTag("r");
			}

            // reduce energy for packet reception of header
            energyReceive(SIGNAL);

			head = ((ClusterHeadMessage *) msg)->getCHead(this->myId);
            if (head != this->myId)
				ev << this->myId << " my cluster head is " << head << "\n";

			if (head != this->myId) // cluster members
            {
				// setup connection from nodes to the cluster head
                cGate *g;
                char gName[32];
                int items;
                //int xpos, ypos;
                cModule *mod;
                int ritems;
                this->cHead = 0;
                this->headId = head;
				
                //xpos = nodePtr[head]->xpos;
                //ypos = nodePtr[head]->ypos;
                mod = (SensorNode *) nodePtr[head];

                items = this->gatev.items();
                ritems = mod->gatev.items();

                // make new gate here
				sprintf(gName, "O_%d", head);
                g = new cGate(gName, 'O');
                this->gatev.addAt(items, g); // position, element
                g->setOwnerModule((cModule *) this, items);
				//hide the arrow from node to cluster head
				g->displayString().setTagArg("o",1,"0");

                // make new gate at other side
                sprintf(gName, "I_%d", this->myId);
                g = new cGate(gName, 'I');
                mod->gatev.addAt(ritems, g); // position, element
                g->setOwnerModule((cModule *) mod, ritems);

                //CHANNEL
                cLinkType *wless = findLink("wless");
                connect((cModule*)this,items,(cLinkType*)wless,(cModule*)mod,ritems);
            }
            else // now this node is cluster head
            {  
                this->cHead = 1;
                this->headId = head;
                this->wasHead++;
                
				// change color of the CH
				if (this->myStatus == STATUS_LIVE)
				//this->displayString().setTagArg("i",1,"red");

                // want to create TDMA in next step
                {
                    this->setDisplayString("p=$xpos,$ypos;i=abstract/router2_vs;r=$ssRange,,blue,1");
					this->displayString().setTagArg("i",1,"red");
					cMessage *cmsg = new cMessage();
                    cmsg->setKind(SMSG_MAKETDMA);
                    scheduleAt(simTime() + INIT_WAIT, cmsg);
                }

                // must stay awake and receive data packets from other nodes
                // reset number of packets received in this frame
                this->nrDataPackets = 0;
                // must collect data and send it to BS
            }
        }
		
		// cluster head announces its TDMA scheme with this message
        // received by nodes in cluster
        if (((ClusterMessage *) msg)->getProto() == CL_TDMA)
        {
            int i;
            int sTime;

            // reset data and counters
            this->myData = 0;   // reset myData
            this->curFrame = 0;

            // reduce energy for packet reception of header
            energyReceive(SIGNAL);

            this->nrFrames = ((TDMAMessage *) msg)->getFrames();
            this->frameTime = ((TDMAMessage *) msg)->getFrameTime();
            this->headId = ((TDMAMessage *) msg)->getSrcAddress();
            ev <<this->myId <<": received TDMA  frameTime: "<<this->frameTime <<"\n";

            // find time slot for this node
            for (i = 0; this->frameTime; i++)
            {
                if (((TDMAMessage *) msg)->getTdma(i) == this->myId &&
                    ((TDMAMessage *) msg)->getTdma(i) != 0)
                {
                    ev << this->myId << ": found time slot " << i+1 << "\n";
                    sTime = i + 1;
                    this->mySlot = i + 1;
                    break;
                }
            }
            //ev << "loop done\n";

            // sleep until received its time slot, then wake up and send data
            {
                cMessage *cmsg = new cMessage();
                cmsg->setKind(SMSG_DATA_NODE2CH);
                scheduleAt(simTime() + sTime, cmsg);
            }
            //ev << "tdma " << this->myId << "done\n";
        }

		// cluster head receives data
        if (((ClusterMessage *) msg)->getProto() == CL_CHDATA)
        {
            //int newHead = 0;    // update in case someone

            // reduce energy for packet reception of data
            energyReceive(DATA);

            // cluster head received data
            this->nrDataPackets++;
            // check if all packets
            if (this->nrDataPackets == this->clusterNodes)
            {
                this->nrDataPackets = 0;
                // two "seconds" until processed data is sent to BS
                ClusterMessage *cmsg = new ClusterMessage();
                cmsg->setKind(SMSG_DATA2RNBS);
				cmsg->setSrcAddress(this->myId);
                scheduleAt(simTime() + 2, cmsg);
				
				// now check if done with this round, this round finish after nrFrames
				this->curFrame++;
				
				if (this->curFrame == this->nrFrames)
				{ 
					ev <<"curFrame: " << curFrame;
					ev << ", cHead: round done\n";
					// reset from being a cluster head
					//this->cHead = 0;
					{
						cMessage *cmsg = new cMessage();
						cmsg->setKind(SMSG_STATUS2BS);
						ev << "node send status to bs: " << this->myId << endl;
						scheduleAt(simTime(), cmsg); // constant
					}
					this->curFrame = 0;
				}
				else 
				{
					ev <<"curFrame: " << curFrame;
					ev <<", goto next frame \n";
				}
            }
        }

		// relay node receiver data
		if (((ClusterMessage *) msg)->getProto() == CL_RNDATA)
		{
			//reduce energy for receiving data
			energyReceive(DATA); // maybe DATA now is larger?
			
			int sender = ((ClusterMessage *) msg)->getDestAddress();

			ev <<"next sender: " << sender << endl;
			{
				ClusterMessage *cmsg = new ClusterMessage();
                cmsg->setKind(SMSG_DATA2RNBS);
				cmsg->setSrcAddress(sender);
                scheduleAt(simTime(), cmsg);
			}
		}
	}
    else //now: msg->isSelfMessage()
    {                           
        if (msg->kind() == SMSG_INIT)
        {
            this->initNodes();
        }
        else if (msg->kind() == SMSG_DATA_NODE2CH)
        {
            int cluaterHead = 0;
			
            // node has been woken up to transmit data to clusterhead
            ev << this->myId << " send data to clusterhead\n";
            sendDataToCHead(cluaterHead);
            this->curFrame++;
            if (this->curFrame < this->nrFrames)
            {
                // sleep until slot time, then wake up and send data
                cMessage *cmsg = new cMessage();
                cmsg->setKind(SMSG_DATA_NODE2CH);
                scheduleAt(simTime() + this->frameTime, cmsg);
            }
            else
            {
                // must wake up to send new status to BS, 
				// so new cluster head can be chosen
                cMessage *cmsg = new cMessage();
                cmsg->setKind(SMSG_STATUS2BS);
                scheduleAt(simTime() + (this->frameTime - this->mySlot), cmsg);
            }
        }
        
		//loop for sending data to RN/BS
		else if (msg->kind() == SMSG_DATA2RNBS)
        {
			int receiver = 0;
			int sender = 0;
			cModule *parent = parentModule();
			int trRange = (int) parent->par("trRange");
			int protocol = (int) parent->par("protocol");
			int showHideRange = (int) parent->par("showHideRange");

			sender = ((ClusterMessage *)msg)->getSrcAddress();
			int D = (int)sqrt(nodePtr[sender]->bsDist);
			ev << "sender: " << sender << ", bsDist: " << D
			   << ", BS trRange is: " << trRange << "\n";
			
			//select routing protocol
			if (protocol == 1)
			{
				// check condition to determine transmitting data to BS or Relay node
				if (D > trRange)
				{
					ev << "now we find relay node \n";
					receiver = selectRelayNode(sender, nodePtr[sender]->xpos, nodePtr[sender]->ypos);
					
					if (receiver == 0) 
					{
						sendData2BS(this->myId);
					}
					else
					{
						if (showHideRange == ON)
						{
							nodePtr[receiver]->setDisplayString("p=$xpos,$ypos;i=misc/node_vs;r=$ssRange,,blue,1");
						}
						nodePtr[receiver]->displayString().setTagArg("i",1,"yellow");

						ev << "receiver found \n";
						sendData2RN(sender, receiver);
					}
				}
				else
				{
					ev <<"now we can send directly data from this RN to bs \n";
					sendData2BS(this->myId);
				}
			}
			else
			{
				ev <<"now we send data to bs directly \n";
				sendData2BS(this->myId);
			}

			// reduce energy for data aggregation
			energyDataAggr(this->clusterNodes); 
			
        }
		else if (msg->kind() == SMSG_MAKETDMA)
        {
            // reset some values
            this->curFrame = 0;
            // now cluster head sends TDMA to all node within its cluster
			//endSimulation();
			this->sendTDMA();
        }
        else if (msg->kind() == SMSG_STATUS2BS)
        {
            //reset some value
			this->myAction = 0;
			this->neighber = 0;
			this->cHead = 0;
			send2BS(this->myId, this->bsId, this->energy, this->myStatus,
                    this->myCluster,this->xpos, this->ypos);
        }
        else
        {
            ev << "got strange self-message!!\n";
        }
    }
    delete(msg);
}

/**************************************************************************************/
void SensorNode::sendData2RN(int sender, int receiver)
{
	ev << sender << " send data to RN " << receiver << "\n";

    Data2RNMessage *dmsg = new Data2RNMessage();
    dmsg->setProto(CL_RNDATA);
	dmsg->setSrcAddress(sender); //  node id
    dmsg->setDestAddress(receiver);
    this->myData++;
	dmsg->setData(this->myData);
    //dmsg->setRelayNode(receiver);
	
	// compute distance between sender and receiver
		int distance = pow(nodePtr[sender]->xpos - nodePtr[receiver]->xpos , 2)
					 + pow(nodePtr[sender]->ypos - nodePtr[receiver]->ypos , 2);
	
	//{			
		//set connection between CH and RN or RN to RN
		cGate *g;
        char gName[32];
        int items;
        int ritems;
        
        items = this->gatev.items();
        ritems = nodePtr[receiver]->gatev.items();

        // make new gate here
		sprintf(gName, "O_%d", receiver);
        g = new cGate(gName, 'O');
        this->gatev.addAt(items, g); // position, element
        g->setOwnerModule((cModule *) this, items);
		//g->displayString().setTagArg("o",1,"0");

        // make new gate at other side
        sprintf(gName, "I_%d", sender);
        g = new cGate(gName, 'I');
        nodePtr[receiver]->gatev.addAt(ritems, g); // position, element
        g->setOwnerModule((cModule *) nodePtr[receiver], ritems);

        //CHANNEL
        cLinkType *wless = findLink("wless");
        connect((cModule*) this,items,(cLinkType*)wless,(cModule*) nodePtr[receiver],ritems);
	//}
	// send data to RN
	{
        char str[32];
		sprintf(str, "O_%d", receiver);
		if (findGate(str) > 1)
        {
			cGate *g = gate(str);
            if (g->isConnected())
            {
                ev << sender << ": sDataRN to " << receiver << "\n";
                send((Data2RNMessage *) dmsg, str);
				this->energyTransmit(DATA, distance);
            }
			else
			{
				ev << "do not connect! \n";
			}
        }
		else 
		{
			ev << "no gate! \n" <<"findGate(str): " << findGate(str) <<"\n";
		}
    }
	//g->displayString().removeTag("o");
}

/**************************************************************************************/
void SensorNode::sendData2BS(int data)
{
    Data2BSMessage *dmsg = new Data2BSMessage();
    dmsg->setProto(CL_CHDATA);
    dmsg->setSrcAddress(this->myId); //  node id
    dmsg->setDestAddress(this->bsId);
    dmsg->setData(this->myData);
    this->myData++;

	// send data to base station
	{
        char str[32];

        sprintf(str, "O_%d", this->bsId);
        if (findGate(str) > 1)
        {
            cGate *g = gate(str);
            if (g->isConnected())
            {
				ev << this->myId << ": sDataBS to" << this->bsId << "\n";
                send((Data2BSMessage *) dmsg, str);
                // reduce energy for data packet to BS
				this->energyTransmit(DATA, this->bsDist);
                
            }
			else
			{
				ev << "do not connect! \n";
			}
        }
		else
		{
			ev << "no gate! \n"; 
		}
    }

}

/**************************************************************************************/
void SensorNode::send2BS(int src, int rec, int energy, 
						  int status, int cluster, int xpos, int ypos)
{

    // nodes send out status to BS
    Status2BSMessage *cmsg = new Status2BSMessage();
    cmsg->setProto(CL_TOBS);
    cmsg->setSrcAddress(this->myId); // node id
    cmsg->setDestAddress(rec);
    cmsg->setStatus(status);
    cmsg->setEnergy(energy);
    cmsg->setCluster(cluster);
    cmsg->setYpos(ypos);
    cmsg->setXpos(xpos);

    //ev << "Node in send2BS: " << this->myId << "\n";
    {
        char str[32];

        sprintf(str, "O_%d", rec);
        if (findGate(str) > 1)
        {
            cGate *g = gate(str);
            if (g->isConnected())
            {
                ev << "Node " << this->myId << " send status to BS: " << rec << "\n";
                send((Status2BSMessage *) cmsg, str);
				// hide the message display here
            }
        }
    }
}
/**************************************************************************************/
void SensorNode::sendTDMA()
{
    ev << "sendTDMA was called" << endl;
	int n;
    int nrPeers = 0; // number of nodes in my cluster
    char str[32];
    int i;
    TDMAMessage *tmsg;
    int items, ritems;
    cGate *g;
    char gName[32];
	
	cModule *parent = parentModule();
	int trRange = (int) parent->par("trRange");

    // create new Message including TDMA slots
    tmsg = new TDMAMessage();
    tmsg->setProto(CL_TDMA);
	tmsg->setTdmaArraySize(this->nrNodes + 3);
    tmsg->setSrcAddress(this->myId);
    tmsg->setDestAddress(BROADCAST);
		
	// compute number of nodes in cluster and update cluster head
    for (n = 2; n < this->nrNodes + 2; n++)
    {
		// compute distance from node to cluster head
        nodePtr[n]->headDist =
               (((nodePtr[n])->xpos - this->xpos)*((nodePtr[n])->xpos - this->xpos))
              +(((nodePtr[n])->ypos - this->ypos)*((nodePtr[n])->ypos - this->ypos));
        //ev << "distance: " << (int)sqrt(nodePtr[n]->headDist) << "\n";

		if (n != this->myId && nodePtr[n]->headId == this->myId)
        {
			// set TDMA slot
			tmsg->setTdma(nrPeers, n); // n is id of node member in CH
			ev << "tdma " << nrPeers << " to node ID : " << n << "\n";
			// set a connection from CH to this node

			items = this->gatev.items();
			ritems = nodePtr[n]->gatev.items();

			// make new gate here (at sender)
			sprintf(gName, "O_%d", nodePtr[n]->myId);
			g = new cGate(gName, 'O');
			this->gatev.addAt(items, g); // position, element
			g->setOwnerModule((cModule *) this, items);

			// make new gate at other side (receiver)
			sprintf(gName, "I_%d", this->myId);
			g = new cGate(gName, 'I');
			nodePtr[n]->gatev.addAt(ritems, g); // position, element
			g->setOwnerModule(nodePtr[n], ritems);

			//CHANNEL
			cLinkType *wless = findLink("wless");
			connect((cModule*) this, items, (cLinkType*) wless, nodePtr[n], ritems);
			// count number of members within cluster
			nrPeers++;
        }
    }
    
	ev << "clusterhead " << this->myId << " have peers: " << nrPeers << "\n";

    tmsg->setFrames(this->nrFrames); //  number of Frames per round
    tmsg->setFrameTime(nrPeers + 2); //  number fo time slot in each frame

    this->clusterNodes = nrPeers;	// number of nodes in this cluster
    // give two slots to cluster head at the end of each round
    tmsg->setTdma(nrPeers, this->myId);
    tmsg->setTdma(nrPeers + 1, this->myId);
    for (i = 0; i < nrPeers; i++)
    {
        int rec = tmsg->getTdma(i);
        sprintf(str, "O_%d", rec);
        if (findGate(str) > 1)
        {
            cGate *g = gate(str);
            if (g->isConnected())
            {
                //ev << this->myId << ": sTDMA to " << rec << "\n";
                sendDirect((TDMAMessage *) tmsg->dup(),0, g);
				//send((TDMAMessage *) tmsg->dup(), str);
                // reduce energy for header
                this->energyTransmit(SIGNAL, nodePtr[rec]->headDist);
            }
        }
        else
            ev << "no gate\n";
    }

    //check if we have 0 peer we do not do anything now, 
	//and we can send our new status to BS to do again
    if (nrPeers == 0)
    {
        ev << "call send2BS\n";
        send2BS(this->myId, this->bsId, this->energy, this->myStatus, 
			    this->myCluster, this->xpos, this->ypos);
    }
}

/************************************************************************************/
int SensorNode::selectRelayNode(int ID, int xPos, int yPos)
{
    cModule *mod;
	cModule *parent = parentModule();
	int trRange = (int) parent->par("trRange");

	struct _adv candidate[NUMBERNODES];
	for (int i = 0; i < NUMBERNODES; i++)
	{
		candidate[i].id = 0;
		candidate[i].kd = 0;
		candidate[i].energy = 0;
	}
	int bestCtr = 0;
	double FMax = 0;
	double Kd = 0;
	double Frn = 0;
	int En = 0;

	int n;
    int relayNode = 0;	// stored ID of relay node

    // compute distance from this node to BS
	int dCHBS =0;
    int x, y; // stored position of base station
	for (i = 1; i <= simulation.lastModuleIndex(); i++)
	{
		//scan the simulation module vector
		mod = (cModule *) simulation.module(i);
		if (strcmp(mod->name(), "bs") == 0)
		{
			x = ((BaseStation *) mod)->xpos;
			y = ((BaseStation *) mod)->ypos;
		}		
	}
	//ev << "BS(x,y) = " << x <<"," << y << endl;
	dCHBS = (x - xPos) * (x - xPos) + (y - yPos) * (y - yPos);
	
	// compute relay node candidate for this cluster
    for (n = 2; n < this->nrNodes + 2; n++)
    {
		// compute distance from relay node candidate to CH/RN
        nodePtr[n]->headDist =
               (((nodePtr[n])->xpos - xPos)*((nodePtr[n])->xpos - xPos))
              +(((nodePtr[n])->ypos - yPos)*((nodePtr[n])->ypos - yPos));
	}
		/*
		ev << "distance RN-CH: " <<nodePtr[n]->myId << "-" << ID << ": " 
			<< (int)sqrt(nodePtr[n]->headDist) << "(m), ";
		ev << "distance RN-BS: " <<nodePtr[n]->myId << "-" <<this->bsId << ": " 
			<< (int)sqrt(nodePtr[n]->bsDist) << "(m)\n";
		ev << "distance CH-BS: " << (int)sqrt(dCHBS) <<"\n";
		ev << "cos(a): " <<nodePtr[n]->myId << "-" << ID << "-BS: " 
			<< nodePtr[n]->cosa << "\n";
		*/
		
		// select relay node from the candidates
	
	for (n = 2; n < this->nrNodes + 2; n++)
	{
		//ev << "node " << n << endl;
		//ev << "      energy = " << nodePtr[n]->energy << endl;
		//ev << "      headDist = " << (int)sqrt(nodePtr[n]->headDist) << endl;
		//ev << "      bsDist = " << (int)sqrt(nodePtr[n]->bsDist) << endl;

		if (nodePtr[n]->energy > 0	&& (int)sqrt(nodePtr[n]->headDist) < trRange 
									&& (int)sqrt(nodePtr[n]->bsDist) < (int)sqrt(dCHBS))
		{
			// compute cos(a)
			nodePtr[n]->cosa = (nodePtr[n]->headDist + dCHBS - nodePtr[n]->bsDist) /
							   (2 * sqrt(nodePtr[n]->headDist) * sqrt(dCHBS));
			
			// compute relaynode function
			candidate[bestCtr].xpos = nodePtr[n]->xpos;
			candidate[bestCtr].ypos = nodePtr[n]->ypos;
			candidate[bestCtr].id = nodePtr[n]->myId;
			candidate[bestCtr].energy = nodePtr[n]->energy;
			candidate[bestCtr].kd = (sqrt(nodePtr[n]->headDist) * nodePtr[n]->cosa ) / 
									 sqrt(nodePtr[n]->bsDist);
			En = candidate[bestCtr].energy;
			Kd = candidate[bestCtr].kd;
			Frn = En * Kd; // result of relay node function
				
				/*
				ev << "RelayNode candidate ID: " << nodePtr[n]->myId 
					<<"(" <<candidate[bestCtr].xpos <<"," <<candidate[bestCtr].ypos <<")"; 
				ev << "with d = (" << (int)sqrt(nodePtr[n]->headDist) << "," 
					<< (int)sqrt(nodePtr[n]->bsDist) <<")\n";
				ev << "cos(a): " <<nodePtr[n]->myId << "-" << ID << "-BS: " 
					<< nodePtr[n]->cosa << "\n";
				ev << "En = " << En << ", Kd = " << Kd << " Frn = " << Frn << "\n\n";
				*/

			if (Frn > FMax)
			{
				FMax = Frn;
				relayNode = candidate[bestCtr].id;
			} 
		
			bestCtr++;
		}
		else
		{
			//ev <<"this node is out of range or not suitable\n\n";
		}
	}
	ev << "Candidate = " << bestCtr << ", RelayNode ID = " << relayNode << endl;
    
	if (relayNode == 0)
    {
        //ev << "no relay node, send directly to base sation!\n";
		ev << "no candidate, increase transmission power" << endl;
		//endSimulation();
    }
	
	// return the result
	ev << "OK, now FMax is: " << FMax;
	ev << " -> my selected relayNode was: " << relayNode <<"\n";
	return relayNode;
}

/************************************************************************************/
void SensorNode::sendDataToCHead(int cluaterHead)
{
    int distance;
	int rec = this->headId;     // this data is sent to the cluster head

    ev << this->myId << "send data to CH: " << rec << "\n";

    DataToCHMessage *dmsg = new DataToCHMessage();
    dmsg->setProto(CL_CHDATA);
    dmsg->setSrcAddress(this->myId); //  node id
    dmsg->setDestAddress(rec);
    dmsg->setData(this->myData);
    this->myData++;
    dmsg->setCluaterHead(cluaterHead);
    {
        char str[32];

        sprintf(str, "O_%d", rec);
        if (findGate(str) > 1)
        {
            cGate *g = gate(str);
            if (g->isConnected())
            {
                ev << this->myId << ": sDataCH to CH: " << rec << "\n";
				ev << this->myId <<"(" <<this->xpos <<"," <<this->ypos <<") to " <<rec 
					<< "(" <<nodePtr[rec]->xpos <<"," <<nodePtr[rec]->ypos <<")\n"; 
				distance = pow(this->xpos - nodePtr[rec]->xpos, 2) + 
						   pow(this->ypos - nodePtr[rec]->ypos, 2);
				ev << "distance: " << (int)sqrt(distance) <<"\n";
                //sendDirect((DataToCHMessage *) dmsg,0, g);
				send((DataToCHMessage *) dmsg, str);
                // reduce energy for data packet
                this->energyTransmit(DATA, distance);
            }
        }
    }
}

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

// energy consumption for reception of bits
void SensorNode::energyReceive(int bits)
{
    double en;

    if (this->myStatus == STATUS_LIVE)
    {
        en = (double) bits *50.0 * NANO;
        ev <<"energy to receive message: "<<bits <<"(bits) is " <<en <<" (microJ)\n";
        this->energy = this->energy - en;
    }
}
/**************************************************************************************/
// energy consumption for transmission of bits over distance dist
// note that dist is already the square
void SensorNode::energyTransmit(int bits, int dist)
{
    double en;
    int transmisionRange;
    cModule *parent = parentModule();
    transmisionRange = (int) parent->par("trRange");
	if (this->energy <= 0) ev <<"out of energy: " << this->myId << endl;
    if (this->myStatus == STATUS_LIVE)
    {
		//if (sqrt(dist) <= transmisionRange)
        //{
			en = ((double) bits *50.0 * NANO) + ((double)bits * 10 * PICO * dist) ;
			ev << "energy to transmit " << bits << "(bits) via " 
			   <<(int) sqrt(dist) << " (m) is " << en << " (microJ)\n";
        //}
        /*
		else
        {
			en = ((double)bits *50.0 * NANO)+((double)bits * 0.0013*PICO*dist*dist) ;
			ev << "energy to transmit " << bits << "(bits) via " 
		       <<(int) sqrt(dist) << " (m) is " << en << " (microJ)\n";
        }*/
		this->energy = this->energy - en;
    }
}
/**************************************************************************************/
// energy consumption for data aggregation (cluster head only)
void SensorNode::energyDataAggr(int signals)
{
    double en;

    if (this->myStatus == STATUS_LIVE)
    {
        en = 5 * NANO * signals;
        ev << "energy to aggregate data: " << en << "\n";
        this->energy = this->energy - en;
    }
}
/**************************************************************************************/
void SensorNode::finish()
{

	//recordScalar("Node:node id", this->myId);
    //recordScalar("Node:lastCluster", this->lastClusterRound);
//#if 0
	if (this->energy > 0)
	{
		recordScalar("Node:energy", (int)this->energy);
	}
	else
	{
		this->energy = 0;
		recordScalar("Node:energy", this->energy);
	}
    //recordScalar("Node:xpos  ", this->xpos);
    //recordScalar("Node:ypos  ", this->ypos);
    recordScalar("Node:wasHead", this->wasHead);
    //recordScalar("Node:status", this->myStatus);


//#endif
}
/**************************************************************************************/
