/* -*-  Mode: C++; c-file-style: "gnu"; indent-tabs-mode:nil; -*- */
/*
 * Copyright (c) 2010 Network Security Lab, University of Washington, Seattle.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation;F
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Author: Sidharth Nabar <snabar@uw.edu>, He Wu <mdzz@u.washington.edu>
 */

#include "ns3/core-module.h"
#include "ns3/network-module.h"
#include "ns3/mobility-module.h"
#include "ns3/config-store-module.h"
#include "ns3/wifi-module.h"
#include "ns3/energy-module.h"
#include "ns3/internet-module.h"

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <set>
#include <exception>

#define NODE_ID int
#define NUMBER_OF_NODES 100
#define NUMBER_OF_SINKS 4

NS_LOG_COMPONENT_DEFINE ("EnergyExample");

using namespace ns3;


namespace ns3 {

        enum MESSAGE_TYPES{
	        TREESETUP = 0,
	        MYPATH = 1,
	        HELLO = 2,
	        DATA = 3,
	        MAXLEVELANNOUNCE = 4,
	        MAXLEVELRESET = 5,
	        LOST = 6,
	        LIFEPROBE = 7,
	        LIFERESPONSE = 8,
	        SINKSHIFT = 9
        };

        class neighbor{
                public:
	                neighbor(){
                        }
	                
                        int ID;
	                int level;
	                int parent;
	                vector <NODE_ID> pathFromSink;
	
	                neighbor& operator=(const neighbor &rhs){
		                this->ID = rhs.ID;
		                this->level = rhs.level;
		                this->parent = rhs.parent;
		                this->pathFromSink = rhs.pathFromSink;
		                return *this;
	                }

	                inline bool operator == (const neighbor &rhs){
		                return (this->ID == rhs.ID);
	                }




                };

        class message{
        public:
	        message(){
		        networkResetTime = -1;

		        this->alive = true;
		        this->flooding =false;
		        this->maxLevel = -1;
		        this->networkResetTime = -1;
		        this->nextSink = -1;
		        this->oldLevel = -1;
		        this->originator = -2;
		        this->originatorLevel = -2;
		        this->path.clear();
		        this->receiver =  -2;
		        this->sender = -2;
		        this->sequence = -2;
		        this->suspect = -2;
		        this->finalReceiver = -2;
		        this->type = HELLO;
		        this->data = "";
		        update=false;
		        x = -1;
		        y = -1;
		        skip = false;
		        newParent = -2;
	        }

            message(uint8_t * packetdata){

            	message();

            	string out(reinterpret_cast<const char *>(packetdata));
            	int counter = 0;

            	int stringlength = out.length();
            	stringlength = stringlength + 1 - 1;
            	char * test = (char*)out.c_str();
            	test[0] = test[0];



					if (atoi(out.substr(counter,1).c_str()) == 0) //1
							this->alive = false;
					else
							this->alive = true;

					counter += 1;//1

					if (atoi(out.substr(counter,1).c_str()) == 0) //1
							this->flooding = false;
					else
							this->flooding = true;

					counter += 1;

					this->maxLevel = atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->networkResetTime =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->nextSink =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->oldLevel =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->originator =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->originatorLevel =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->receiver =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->sender =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->sequence =  atoi(out.substr(counter,6).c_str());

					counter += 6;

					this->suspect =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					int mytype = (atoi(out.substr(counter,2).c_str()));

					switch (mytype){

					case 0:
						type = TREESETUP;
						break;
					case 1:
						type = MYPATH;
						break;
					case 2:
						type = HELLO;
						break;
					case 3:
						type = DATA;
						break;
					case 4:
						type = MAXLEVELANNOUNCE;
						break;
					case 5:
						type = MAXLEVELRESET;
						break;
					case 6:
						type = LOST;
						break;
					case 7:
						type = LIFEPROBE;
						break;
					case 8:
						type = LIFERESPONSE;
						break;
					case 9:
						type = SINKSHIFT;
						break;

					}

					counter += 2;

					this->finalReceiver =  atoi(out.substr(counter,3).c_str());

					counter += 3;

					if (atoi(out.substr(counter,1).c_str()) == 0) //1
							this->update = false;
					else
							this->update = true;

					counter += 1;

					this->x = atoi(out.substr(counter,3).c_str());

					counter += 3;

					this->y = atoi(out.substr(counter,3).c_str());

					counter += 3;

					if (atoi(out.substr(counter,1).c_str()) == 0) //1
							this->skip = false;
					else
							this->skip = true;

					counter += 1;

					this->newParent = atoi(out.substr(counter,3).c_str());

					counter += 3;

					int pathsize = atoi(out.substr(counter,4).c_str());

					counter += 4;

					if (pathsize > 0){

						for (int i = 0; i < pathsize ; i++){

							int step = atoi(out.substr(counter,3).c_str());
							counter += 3;
							path.push_back(step);
						}

					}



					int childrensize = atoi(out.substr(counter,4).c_str());

					counter += 4;

					if (childrensize > 0){

						for (int i = 0; i < childrensize ; i++){

							int step = atoi(out.substr(counter,3).c_str());
							counter += 3;
							children.insert(step);
						}

					}

					int neighborsize = atoi(out.substr(counter,4).c_str());

					counter += 4;

					if (neighborsize > 0){

						for (int i = 0; i < neighborsize ; i++){

							int step = atoi(out.substr(counter,3).c_str());
							counter += 3;
							neighbors.insert(step);
						}

					}

					data = out.substr(counter);

	        }

	        bool alive;
	        bool flooding;
	        int maxLevel;
	        int networkResetTime;
	        int nextSink;
	        int oldLevel;
	        int originator;
	        int originatorLevel;
	        //int path[30];
	        vector<NODE_ID> path;
	        int receiver; //ALL == -1
	        int sender; 
	        int sequence; //Random num for message id
	        int suspect;
	        string data;
	        int finalReceiver;
	        bool update;
	        int x;
	        int y;
	        int newParent;
	        bool skip;
	        MESSAGE_TYPES type;
	        set<NODE_ID> children;
	        set<NODE_ID> neighbors;

	        message& operator=(const message &rhs){
		        this->alive = rhs.alive;
		        this->flooding = rhs.flooding;
		        this->maxLevel = rhs.maxLevel;
		        this->networkResetTime = rhs.networkResetTime;
		        this->nextSink = rhs.nextSink;
		        this->oldLevel = rhs.oldLevel;
		        this->originator = rhs.originator;
		        this->originatorLevel = rhs.originatorLevel;
		        this->path = rhs.path;
		        this->receiver = rhs.receiver;
		        this->sender = rhs.sender;
		        this->sequence = rhs.sequence;
		        this->suspect = rhs.suspect;
		        this->type = rhs.type;
		        this->data = rhs.data;
		        this->finalReceiver = rhs.finalReceiver;
		        update = rhs.update;
		        x = rhs.x;
		        y = rhs.y;
		        children = rhs.children;
		        skip = rhs.skip;
		        newParent = rhs.newParent;
		        neighbors = rhs.neighbors;
		        return *this;
	        }

                string ZeroPadNumber(int num, int len)
                {
	                stringstream ss;
	
	                // the number is converted to string with the help of stringstream
	                ss << num; 
	                string ret;
	                ss >> ret;
	
	                // Append zero chars
	                int str_length = ret.length();
	                for (int i = 0; i < len - str_length; i++)
		                ret = "0" + ret;
	                return ret;
                }

                string Output(){
                        string out = "";
                        
                        if (this->alive == true) //1 0
                                out += "1";
                        else
                                out += "0";

                        if (this->flooding == true) //1 1
                                out += "1";
                        else
                                out += "0";
                        
                        if (this->maxLevel >= 0) //3 2
                                 out += ZeroPadNumber(this->maxLevel,3); 
                        else
                                 out += ZeroPadNumber(0,3);
                        
                        if (this->networkResetTime >= 0) //3 5
                                 out += ZeroPadNumber(this->networkResetTime,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->nextSink >= 0) //3 8
                                 out += ZeroPadNumber(this->nextSink,3); 
                        else
                                 out += ZeroPadNumber(0,3);
                        
                        if (this->oldLevel >= 0) //3 12
                                 out += ZeroPadNumber(this->oldLevel,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->originator >= 0) //3 15
                                 out += ZeroPadNumber(this->originator,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->originatorLevel >= 0) //3 18
                                 out += ZeroPadNumber(this->originatorLevel,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->receiver >= 0) //3 21
                                 out += ZeroPadNumber(this->receiver,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->sender >= 0) //3 24
                                 out += ZeroPadNumber(this->sender,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->sequence >= 0) //6 27
                                 out += ZeroPadNumber(this->sequence,6); 
                        else
                                 out += ZeroPadNumber(0,6);

                        if (this->suspect >= 0) //3 33
                                 out += ZeroPadNumber(this->suspect,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->type >= 0) //2 36
                                 out += ZeroPadNumber(this->type,2); 
                        else
                                 out += ZeroPadNumber(0,2);

                        if (this->finalReceiver >= 0) //3 38
                                 out += ZeroPadNumber(this->finalReceiver,3); 
                        else
                                 out += ZeroPadNumber(0,3);
		        
                        if (this->update == true) //1 41
                                out += "1";
                        else
                                out += "0";

                        if (this->x >= 0) //3 42
                                 out += ZeroPadNumber(this->x,3); 
                        else
                                 out += ZeroPadNumber(0,3);

                        if (this->y >= 0) //3 45
                                 out += ZeroPadNumber(this->y,3); 
                        else
                                 out += ZeroPadNumber(0,3);
                
                        if (this->skip == true) //1 49
                                out += "1";
                        else
                                out += "0";

                        if (this->newParent >= 0) //3 50
                                 out += ZeroPadNumber(this->newParent,3); 
                        else
                                 out += ZeroPadNumber(0,3);


                        //Path
                        if (this->path.size() >= 0) //4 length of path data number of entries * 3       4 53
                                 out += ZeroPadNumber(this->path.size(),4);
                        else
                                 out += ZeroPadNumber(0,4);

                        int p = path.size();

	                for (int i = 0; i < p ; i++){
		                int val = path[i];
		                out += ZeroPadNumber(val,3);
	                }

                        
                        //Children
                        if (this->children.size() >= 0) //4 length of path data number of entries * 3
                                 out += ZeroPadNumber(this->children.size(),4);
                        else
                                 out += ZeroPadNumber(0,4);

                        p = children.size();

                        set<NODE_ID>::iterator it;
                        for( it = children.begin(); it != children.end(); it++ ) {
                                //cout << it-＞num << "\t" << it-＞name << endl;
                                int val = *it;
		                out += ZeroPadNumber(val,3);
                        }

	               // for (int i = 0; i < p ; i++){
		        //        int val = children[i];
		        //        out += ZeroPadNumber(val,3);
	                //}

                        //Neighbors
                        if (this->neighbors.size() >= 0) //4 length of path data number of entries * 3
                                 out += ZeroPadNumber(this->neighbors.size() * 3,4); 
                        else
                                 out += ZeroPadNumber(0,4);

                        p = neighbors.size();

	                //for (int i = 0; i < p ; i++){
		        //        int val = neighbors[i];
		        //        out += ZeroPadNumber(val,3);
	                //}
                        
                        for( it = neighbors.begin(); it != neighbors.end(); it++ ) {
                                //cout << it-＞num << "\t" << it-＞name << endl;
                                int val = *it;
		                out += ZeroPadNumber(val,3);
                        }

                        //Data
                        //if (this->data >= 0) //4 length of path data number of entries * 3
                       //          out += ZeroPadNumber(this->data.size(),4); 
                        //else
                         //        out += ZeroPadNumber(0,4);

                        //p = neighbors.size();

	                //for (int i = 0; i < p ; i++){
		                //int val = neighbors[i];
		        out += data;
	                //}


                        return out;


                }                


        };

        class WSNBaseNode: public Node{
        public:
        	WSNBaseNode(){

        	}
        	int NodeID;
            int x;
            int y;

        };

        class WSNSensorNode : public WSNBaseNode
        {

           public:     

           WSNSensorNode(){
           
              //Test
              isDead = false;
              level = -1;
                
           }

           
           NodeContainer neighbors;
           set<NODE_ID> children;
           NodeContainer childrenNodes;
           bool isDead; 

           Ptr<WSNBaseNode> Parent;
           int parent;
           int level;
           vector<neighbor> backupParents;
           vector<NODE_ID> pathFromSink;
           bool disconnected;
           int power;

           vector<neighbor> oneHopNeighbors;

                int getNewSeq(){
                        return 999999; //should be a random int.
                }

        };


        class WSNSinkNode : public WSNBaseNode{
           public:
           WSNSinkNode(){
           
              Active = false;

           }

           NodeContainer childrenNodes;
           set<NODE_ID> children;
           NodeContainer neighbors;
           int NodeID;
           bool Active;
           vector<neighbor> oneHopNeighbors;

        };

        class SinkApplication:public Application{
                
        protected:
          virtual void DoDispose (void){
                Application::DoDispose ();
          }

        private:

          virtual void StartApplication (void){
                if (m_socket == 0)
                    {
                      NS_LOG_UNCOND ("Sink Make Socket");
                      TypeId tid = TypeId::LookupByName ("ns3::UdpSocketFactory");
                      m_socket = Socket::CreateSocket (GetNode (), tid);
                      InetSocketAddress local = InetSocketAddress (Ipv4Address::GetAny (), 80);
                      m_socket->Bind (local);
                      NS_LOG_UNCOND ("Sink End Socket");
                    }

                  m_socket->SetRecvCallback (MakeCallback (&ns3::SinkApplication::HandleReceive, this));

                  Ptr<WSNSinkNode> SinkNode = DynamicCast <WSNSinkNode> (GetNode());

                  if (SinkNode->Active == true){
                	  	message m;
						m.type = TREESETUP;
						m.sender = SinkNode->NodeID;
						m.originator = m.sender;
						m.flooding = true;
						m.sequence = 1;
						m.path.push_back(SinkNode->NodeID);
						SendToChildren(m);
                  }


                 
          }

          virtual void StopApplication (void){
                  if (m_socket != 0) 
                  {
                    m_socket->Close ();
                    m_socket->SetRecvCallback (MakeNullCallback<void, Ptr<Socket> > ());
                  }
          }

          void SendToChildren (message m){
                Ptr<WSNSinkNode> SinkNode = DynamicCast <WSNSinkNode> (GetNode());

                Ptr<Ipv4> cipv4 = SinkNode->GetObject<Ipv4>(); 
                Ipv4InterfaceAddress ciaddr = cipv4->GetAddress (1,0);
               
                for (unsigned int i = 0; i < SinkNode->childrenNodes.GetN(); i++){
                	Ptr<WSNBaseNode> cnode = DynamicCast <WSNBaseNode> (SinkNode->childrenNodes.Get(i));

                     Ptr<Ipv4> ipv4 = SinkNode->childrenNodes.Get(i)->GetObject<Ipv4>(); 
                     Ipv4InterfaceAddress iaddr = ipv4->GetAddress (1,0);

                     //NS_LOG_UNCOND (ciaddr.GetLocal()<<  " Sending Packet to: "<<iaddr.GetLocal());
                     NS_LOG_UNCOND (SinkNode->NodeID <<  " Sending Packet to: "<< cnode->NodeID);

                     m_socket->Connect (InetSocketAddress (iaddr.GetLocal(),80)); 

                     string mess_output = m.Output();
                     Ptr<Packet> pkt1 = Create<Packet> (reinterpret_cast<const uint8_t*> (mess_output.c_str()), mess_output.length());


                     m_socket->Send (pkt1);
                }
          }

          void SendToChildren (void){
                message m;               
                SendToChildren (m);
                 
          }


          void HandleReceive (Ptr<Socket> socket){
                Ptr<Packet> packet;
                  Address from;
                  while (packet = socket->RecvFrom (from))
                    {
                      if (InetSocketAddress::IsMatchingType (from))
                        {
                          NS_LOG_INFO ("Received " << packet->GetSize () << " bytes from " <<
                                       InetSocketAddress::ConvertFrom (from).GetIpv4 ());





                          packet->RemoveAllPacketTags ();
                          packet->RemoveAllByteTags ();

                          uint8_t * buff = new uint8_t[packet->GetSerializedSize()];

                          packet->CopyData(buff, packet->GetSerializedSize());

                          message m(buff);

                          NS_LOG_UNCOND (m.sender <<  " got Packet from: "<< m.receiver);

                          //message m;

                          switch (m.type){
		
		                case TREESETUP:
			                break;
		                case MYPATH:
			                //a3(m);
			                break;
		                case HELLO:
			                //a2(m);
			                break;
		                case DATA:
			                //dataProcessor(m);
			                break;
		                case MAXLEVELANNOUNCE:
			
			                break;
		                case MAXLEVELRESET:
			                break;
		                case LOST:
			                //a5(m);
			                break;
		                case LIFEPROBE:
			
			                break;
		                case LIFERESPONSE:
			
			                break;
		                case SINKSHIFT:
			                //a8(m);
			                break;

	
	                }

                          NS_LOG_LOGIC ("Echoing packet");
                          socket->SendTo (packet, 0, from);
                        }
                    }
          }

          Ptr<Socket> m_socket;
          Ipv4Address m_peerAddress;
          Address m_local;
        };

        class SensorApplication:public Application{
        protected:
          virtual void DoDispose (void){
                Application::DoDispose ();
          }

        private:

          virtual void StartApplication (void){
                if (m_socket == 0)
                    {
                        
                      TypeId tid = TypeId::LookupByName ("ns3::UdpSocketFactory");
                      m_socket = Socket::CreateSocket (GetNode (), tid);
                      InetSocketAddress local = InetSocketAddress (Ipv4Address::GetAny (), 80);
                      m_socket->Bind (local);
                        
                    }

                  m_socket->SetRecvCallback (MakeCallback (&ns3::SensorApplication::HandleReceive, this));
          }

          virtual void StopApplication (void){
                  if (m_socket != 0) 
                  {
                    m_socket->Close ();
                    m_socket->SetRecvCallback (MakeNullCallback<void, Ptr<Socket> > ());
                  }
          }

           void SendToChildren (void){
                 message m;               
                SendToChildren (m);
           
          }

          void SendToChildren (message m){
                     Ptr<WSNSensorNode> SensorNode = DynamicCast <WSNSensorNode> (GetNode());
                Ptr<Ipv4> cipv4 = SensorNode->GetObject<Ipv4>(); 
                Ipv4InterfaceAddress ciaddr = cipv4->GetAddress (1,0);

                for (unsigned int i = 0; i < SensorNode->childrenNodes.GetN(); i++){


                	Ptr<WSNBaseNode> cnode = DynamicCast <WSNBaseNode> (SensorNode->childrenNodes.Get(i));
                	NS_LOG_UNCOND (SensorNode->NodeID <<  " Sending Packet to: "<< cnode->NodeID);

                     Ptr<Ipv4> ipv4 = SensorNode->childrenNodes.Get(i)->GetObject<Ipv4>(); 
                     Ipv4InterfaceAddress iaddr = ipv4->GetAddress (1,0);
                        //NS_LOG_UNCOND (ciaddr.GetLocal()<<  " Sending Packet to: "<<iaddr.GetLocal());
                        m_socket->Connect (InetSocketAddress (iaddr.GetLocal(),80)); 

                        string mess_output = m.Output();
						Ptr<Packet> pkt1 = Create<Packet> (reinterpret_cast<const uint8_t*> (mess_output.c_str()), mess_output.length());


						m_socket->Send (pkt1);
                     //m_socket->Send (Create<Packet> (200));
                }
          } 

          void SendToParent(void){
                message m;               
                SendToChildren (m);
          }

          void SendToParent(message m){
                     Ptr<WSNSensorNode> SensorNode = DynamicCast <WSNSensorNode> (GetNode());


                     Ptr<WSNBaseNode> cnode = DynamicCast <WSNBaseNode> (SensorNode->Parent);
                     NS_LOG_UNCOND (SensorNode->NodeID <<  " Sending Packet Parent: "<< cnode->NodeID);


                     Ptr<Ipv4> ipv4 = SensorNode->Parent->GetObject<Ipv4>();
                     Ipv4InterfaceAddress iaddr = ipv4->GetAddress (1,0);
                     m_socket->Connect (InetSocketAddress (iaddr.GetLocal(),80));
                     //m_socket->Send (Create<Packet> (200));

                     string mess_output = m.Output();
					 Ptr<Packet> pkt1 = Create<Packet> (reinterpret_cast<const uint8_t*> (mess_output.c_str()), mess_output.length());


					 m_socket->Send (pkt1);
          }

                void a3(message m){
	                int p = m.path.size();


	                //See if path contains redundant paths wich make a loop;
	                set<int> dups; 
	                pair<set<int>::iterator,bool> ret;
	                for (int i = 0; i < p ; i++){
		                ret = dups.insert(m.path[i]);
		                if (ret.second == false){
			                //cout<<"Duplicates in Path"<<endl;
			                return;
		                }
	                }

	                ret = dups.insert(DynamicCast <WSNSensorNode> (GetNode())->NodeID);
	                if (ret.second == false){
		                //cout<<"Duplicates in Path"<<endl;
		                return;
	                }

	                ///////////////////////////////
	                if (DynamicCast <WSNSensorNode> (GetNode())->level != -1 && p >= DynamicCast <WSNSensorNode> (GetNode())->level){
		                neighbor n;
		                n.ID = m.sender;
		                n.pathFromSink = m.path;
		                n.level = m.path.size() - 1;

		                vector<NODE_ID>::iterator it = m.path.end();
		                if (m.path.size() > 0){
				                it--;

			                n.parent = *it;
			                DynamicCast <WSNSensorNode> (GetNode())->backupParents.push_back(n);
		                }


		                return;
	                }

	                //i added the small protion below to weed out any reception of duplicate messages. It might not be needed but its here for safty. 
	                vector<int>::iterator it;
	                vector<int>::iterator it2;
	                it = m.path.begin();
	                it2 = DynamicCast <WSNSensorNode> (GetNode())->pathFromSink.begin();
	                while (true){

		                if (it == m.path.end()){
			                return;
		                }
		                if (it2 == DynamicCast <WSNSensorNode> (GetNode())->pathFromSink.end()){
			                break;
		                }

		                if ((*it) != (*it2)){
			                break;
		                }

		                it++;
		                it2++;
	                }


	                //Testing
	                if (m.path.size() > 0 && DynamicCast <WSNSensorNode> (GetNode())->pathFromSink.size() > 0 && m.originator == DynamicCast <WSNSensorNode> (GetNode())->pathFromSink[0] && m.path.size() >= DynamicCast <WSNSensorNode> (GetNode())->pathFromSink.size()){
		                return;
	                }


	                //cout<<"TreeSetup: "<<ID<<" SINK:"<<m.originator<<endl;

	                //sets level and then removes it if it is a child. 
	                DynamicCast <WSNSensorNode> (GetNode())->level = p;

	                DynamicCast <WSNSensorNode> (GetNode())->pathFromSink = m.path;
	                DynamicCast <WSNSensorNode> (GetNode())->pathFromSink.push_back(DynamicCast <WSNSensorNode> (GetNode())->NodeID);
	                DynamicCast <WSNSensorNode> (GetNode())->parent = m.sender;

	                //For ns3 we need to find the parent from the list of neighbornodes and load Parent
	                Ptr<WSNSensorNode> CurSensorNode = DynamicCast <WSNSensorNode> (GetNode());

	                for (unsigned int i = 0; i < CurSensorNode->neighbors.GetN(); i++){
	                	  Ptr<WSNSensorNode> NeighborSensorNode = DynamicCast <WSNSensorNode> (CurSensorNode->neighbors.Get(i));

	                	  if (NeighborSensorNode == NULL){

	                		  Ptr<WSNSinkNode> NeighborSinkNode = DynamicCast <WSNSinkNode> (CurSensorNode->neighbors.Get(i));

	                		  if (NeighborSinkNode == NULL){
	                			  continue;
	                		  }else{

	    	                	  try
	    	                	  {
	    	                		  int tempnodeid = NeighborSinkNode->NodeID ;
	    	                		  if (tempnodeid == m.sender){
	    								  CurSensorNode->Parent = NeighborSinkNode;
	    								  break;
	    							  }
	    	                	  }catch (exception& e)
	    	                	  {
	    	                		  cout << e.what() << endl;
	    	                	  }

	    	                	  continue;
	                		  }

	                	  }

	                	  try
	                	  {
	                		  int tempnodeid = NeighborSensorNode->NodeID ;
	                		  int sender = m.sender;
	                		  if (tempnodeid == sender){
								  CurSensorNode->Parent = NeighborSensorNode;
								  int j = 0;
								  j += 1;
								  j = 0;
							  }

	                	  }catch (exception& e)
	                	  {
	                		  cout << e.what() << endl;
	                	  }



					 }

	                DynamicCast <WSNSensorNode> (GetNode())->children.erase(m.sender);
	                DynamicCast <WSNSensorNode> (GetNode())->disconnected = false;
	                //Send to kids

	                message m1;

	                m1.type = TREESETUP;
	                m1.originator  = m.originator;
	                m1.sender = DynamicCast <WSNSensorNode> (GetNode())->NodeID;
	                m1.path = DynamicCast <WSNSensorNode> (GetNode())->pathFromSink;
	                m1.sequence = m.sequence;
	                m1.flooding = true;
	                SendToChildren(m1);


	                DynamicCast <WSNSensorNode> (GetNode())->power -= .000066912;

	                //sending to parent;
		                message m2;

		                m2.type = MYPATH;
		                m2.originator  = DynamicCast <WSNSensorNode> (GetNode())->NodeID;
		                m2.sender = DynamicCast <WSNSensorNode> (GetNode())->NodeID;
		                m2.path = DynamicCast <WSNSensorNode> (GetNode())->pathFromSink;
		                m2.sequence = DynamicCast <WSNSensorNode> (GetNode())->getNewSeq();//m.sequence;
		                m2.flooding = true;
		                m2.x = DynamicCast <WSNSensorNode> (GetNode())->x;
		                m2.y = DynamicCast <WSNSensorNode> (GetNode())->y;
		                m2.children =  DynamicCast <WSNSensorNode> (GetNode())->children;
		                m2.receiver = DynamicCast <WSNSensorNode> (GetNode())->Parent->NodeID;
		                for (unsigned int i = 0; i < DynamicCast <WSNSensorNode> (GetNode())->oneHopNeighbors.size();i++){
			                if (DynamicCast <WSNSensorNode> (GetNode())->oneHopNeighbors.at(i).ID < NUMBER_OF_NODES)
				                m2.neighbors.insert(DynamicCast <WSNSensorNode> (GetNode())->oneHopNeighbors.at(i).ID);
		                }
		                SendToParent(m2);

		                DynamicCast <WSNSensorNode> (GetNode())->power -= .000066912;

                }

          void HandleReceive (Ptr<Socket> socket){
                Ptr<Packet> packet;
                  Address from;
                  while (packet = socket->RecvFrom (from))
                    {
                      if (InetSocketAddress::IsMatchingType (from))
                        {
                          NS_LOG_INFO ("Received " << packet->GetSize () << " bytes from " <<
                                       InetSocketAddress::ConvertFrom (from).GetIpv4 ());

                          packet->RemoveAllPacketTags ();
                          packet->RemoveAllByteTags ();

                          uint8_t * buff = new uint8_t[packet->GetSerializedSize()];

                          packet->CopyData(buff, packet->GetSerializedSize());

                          message m(buff);

                        switch (m.type){
	
	                        case TREESETUP:
		                        a3(m);
		                        break;
	                        case MYPATH:
		                        //a4(m);
		                        break;
	                        case HELLO:
		                        //a2(m);
		                        break;
	                        case DATA:
		                        //a7(m);
		                        break;
	                        case MAXLEVELANNOUNCE:
		                        //a5(m);
		                        break;
	                        case MAXLEVELRESET:
		                        break;
	                        case LOST:
		                        //a9(m);
		                        break;
	                        case LIFEPROBE:
		                        //a10(m);
		                        break;
	                        case LIFERESPONSE:
		                        //a11(m);
		                        break;
	                        case SINKSHIFT:
		                        //a13(m);
		                        break;


                        }


                          //NS_LOG_LOGIC ("Echoing packet");
                          //socket->SendTo (packet, 0, from);
                       // SendToChildren();
                        }
                    }
          }

          Ptr<Socket> m_socket;
          Ipv4Address m_peerAddress;
          Address m_local;
        };
        
   

}

/**
 * \param socket Pointer to socket.
 *
 * Packet receiving sink.
 */
void
ReceivePacket (Ptr<Socket> socket)
{
  Ptr<Packet> packet;
  Address from;
  while (packet = socket->RecvFrom (from))
    {
      if (packet->GetSize () > 0)
        {
          InetSocketAddress iaddr = InetSocketAddress::ConvertFrom (from);
          NS_LOG_UNCOND ("--\nReceived one packet! Socket: "<< iaddr.GetIpv4 ()
                                                            << " port: " << iaddr.GetPort () << " at time = " <<
                         Simulator::Now ().GetSeconds () << "\n--");


           socket->Send (Create<Packet> (200));
            NS_LOG_UNCOND ("--\nSent one packet! Socket: "<< " Out REC."
                                                            << " port: " << "80" << " at time = " <<
                         Simulator::Now ().GetSeconds () << "\n--");
          //cast iaddr to void, to suppress 'iaddr' set but not used compiler warning
          //in optimized builds
          (void) iaddr;
        }
    }
}

/**
 * \param socket Pointer to socket.
 * \param pktSize Packet size.
 * \param n Pointer to node.
 * \param pktCount Number of packets to generate.
 * \param pktInterval Packet sending interval.
 *
 * Traffic generator.
 */
static void
GenerateTraffic (Ptr<Socket> socket, uint32_t pktSize, Ptr<Node> n,
                 uint32_t pktCount, Time pktInterval)
{
  if (pktCount > 0)
    {
      socket->Send (Create<Packet> (pktSize));

      Simulator::Schedule (pktInterval, &GenerateTraffic, socket, pktSize, n,
                           pktCount - 1, pktInterval);

      Ptr<Ipv4> ipv4 = n->GetObject<Ipv4>(); 
      Ipv4InterfaceAddress iaddr = ipv4->GetAddress (1,0);       

      //InetSocketAddress iaddr = InetSocketAddress::ConvertFrom (from);
      NS_LOG_UNCOND ("--\nSent one packet! Socket: "<< iaddr.GetLocal ()
                                                            << " port: " << "80" << " at time = " <<
                         Simulator::Now ().GetSeconds () << "\n--");


      //NS_LOG_UNCOND ("--\nSent one packet! Socket: "<< "Test"
      //                                                      << " port: " << "80" << " at time = " <<
       //                  Simulator::Now ().GetSeconds () << "\n--");
    }
  else
    {
      socket->Close ();
    }
}

/// Trace function for remaining energy at node.
void
RemainingEnergy (double oldValue, double remainingEnergy)
{
  NS_LOG_UNCOND (Simulator::Now ().GetSeconds ()
                 << "s Current remaining energy = " << remainingEnergy << "J");
}

/// Trace function for total energy consumption at node.
void
TotalEnergy (double oldValue, double totalEnergy)
{
  NS_LOG_UNCOND (Simulator::Now ().GetSeconds ()
                 << "s Total energy consumed by radio = " << totalEnergy << "J");
}


void loadOneHopNeighbors(NodeContainer sinkNodes, NodeContainer SensorNodes,int id){

   int RADIO_RANGE = 15;
   //int NUMBER_OF_NODES = 100;
   //int NUMBER_OF_SINKS = 4;

   if (id >= NUMBER_OF_NODES){
        for (int i = 0; i<NUMBER_OF_NODES; i++){
                
                Ptr<WSNSensorNode> SensorNode = DynamicCast <WSNSensorNode> (SensorNodes.Get(i));
                Ptr<MobilityModel> Sensormb = SensorNode->GetObject<MobilityModel>();
                Vector SensorPosition = Sensormb->GetPosition(); 

                for (int j = 0; j < NUMBER_OF_SINKS;j++){

                        
                        Ptr<WSNSinkNode> SinkNode = DynamicCast <WSNSinkNode> (sinkNodes.Get(j));
                        Ptr<MobilityModel> Sinkmb = SinkNode->GetObject<MobilityModel>();
                        Vector SinkPosition = Sinkmb->GetPosition(); 

                        //distance
			double r2 = sqrt(pow(SensorPosition.x - SinkPosition.x, 2.0) + pow(SensorPosition.y - SinkPosition.y, 2.0));  
                        //NS_LOG_UNCOND ("r2: " <<r2 <<" Sinkx:"<<SinkPosition.x << " Sinky:" << SinkPosition.y);

                        if (r2 <= RADIO_RANGE && SinkNode->children.find(SensorNode->NodeID) == SinkNode->children.end()){

                                SinkNode->neighbors.Add(SensorNode);
                                SinkNode->childrenNodes.Add(SensorNode); 
                                SinkNode->children.insert(SensorNode->NodeID);
                                SensorNode->neighbors.Add(SinkNode);

                                if (SinkNode->Active){
                                        
                                        //SensorNode->children.Add(SinkNode);
                                        SensorNode->Parent = SinkNode;                                        

                                }

                        }            
                
                }

        }     
   }else{

        Ptr<WSNSensorNode> SensorNodeID = DynamicCast <WSNSensorNode> (SensorNodes.Get(id));
        Ptr<MobilityModel> SensorIdmb = SensorNodeID->GetObject<MobilityModel>();
        Vector SensorPositionID = SensorIdmb->GetPosition(); 
        
        for (int i = 0; i<NUMBER_OF_NODES; i++){
        
                if (i != id){

                        Ptr<WSNSensorNode> CurSensorNode = DynamicCast <WSNSensorNode> (SensorNodes.Get(i));
                        Ptr<MobilityModel> CurSensormb = CurSensorNode->GetObject<MobilityModel>();
                        Vector CurSensorPosition = CurSensormb->GetPosition(); 

                        //distance
			double r = sqrt(pow(CurSensorPosition.x - SensorPositionID.x, 2.0) + pow(CurSensorPosition.y - SensorPositionID.y, 2.0)); 
                        if (r <= RADIO_RANGE  && SensorNodeID->children.find(CurSensorNode->NodeID) == SensorNodeID->children.end() ){
                                SensorNodeID->neighbors.Add(CurSensorNode);
                                SensorNodeID->children.insert(CurSensorNode->NodeID);
                                SensorNodeID->childrenNodes.Add(CurSensorNode); 

                                CurSensorNode->neighbors.Add(SensorNodeID);
                                CurSensorNode->childrenNodes.Add(SensorNodeID);
                                CurSensorNode->children.insert(SensorNodeID->NodeID);
                        }                 
                
                }        

        }


   }


}


int
main (int argc, char *argv[])
{
  /*
  LogComponentEnable ("EnergySource", LOG_LEVEL_DEBUG);
  LogComponentEnable ("BasicEnergySource", LOG_LEVEL_DEBUG);
  LogComponentEnable ("DeviceEnergyModel", LOG_LEVEL_DEBUG);
  LogComponentEnable ("WifiRadioEnergyModel", LOG_LEVEL_DEBUG);
   */

  std::string phyMode ("DsssRate1Mbps");
  double Prss = -80;            // dBm
  uint32_t PpacketSize = 200;   // bytes
  bool verbose = false;

  // simulation parameters
  uint32_t numPackets = 10000;  // number of packets to send
  double interval = 1;          // seconds
  double startTime = 0.0;       // seconds
  double distanceToRx = 100.0;  // meters
  /*
   * This is a magic number used to set the transmit power, based on other
   * configuration.
   */
  double offset = 81;

  CommandLine cmd;
  cmd.AddValue ("phyMode", "Wifi Phy mode", phyMode);
  cmd.AddValue ("Prss", "Intended primary RSS (dBm)", Prss);
  cmd.AddValue ("PpacketSize", "size of application packet sent", PpacketSize);
  cmd.AddValue ("numPackets", "Total number of packets to send", numPackets);
  cmd.AddValue ("startTime", "Simulation start time", startTime);
  cmd.AddValue ("distanceToRx", "X-Axis distance between nodes", distanceToRx);
  cmd.AddValue ("verbose", "Turn on all device log components", verbose);
  cmd.Parse (argc, argv);

  // Convert to time object
  Time interPacketInterval = Seconds (interval);

  // disable fragmentation for frames below 2200 bytes
  Config::SetDefault ("ns3::WifiRemoteStationManager::FragmentationThreshold",
                      StringValue ("2200"));
  // turn off RTS/CTS for frames below 2200 bytes
  Config::SetDefault ("ns3::WifiRemoteStationManager::RtsCtsThreshold",
                      StringValue ("2200"));
  // Fix non-unicast data rate to be the same as that of unicast
  Config::SetDefault ("ns3::WifiRemoteStationManager::NonUnicastMode",
                      StringValue (phyMode));

  NodeContainer c;
  //c.Create (100);     // create 2 nodes

  for (int i = 0; i < 100; i++){
  
      Ptr<WSNSensorNode> SensorNode = CreateObject<WSNSensorNode>();
      SensorNode->NodeID = i;
      c.Add(SensorNode);
  
  }

  
  

  NodeContainer SensorNodes;

  for (int i = 0; i < 100; i++){
     SensorNodes.Add (c.Get (i));
  }

  NodeContainer SinkNodes;
  for (int i = 0; i < 4; i++){
  
      Ptr<WSNSinkNode> SinkNode = CreateObject<WSNSinkNode>();
      SinkNode->NodeID = i+100;
      if (i == 0){
        SinkNode->Active = true;
      }
      SinkNodes.Add(SinkNode);
  
  }


  

  // The below set of helpers will help us to put together the wifi NICs we want
  WifiHelper wifi;
  if (verbose)
    {
      wifi.EnableLogComponents ();
    }
  wifi.SetStandard (WIFI_PHY_STANDARD_80211b);

  /** Wifi PHY **/
  /***************************************************************************/
  YansWifiPhyHelper wifiPhy = YansWifiPhyHelper::Default ();
  wifiPhy.Set ("RxGain", DoubleValue (-10));
  wifiPhy.Set ("TxGain", DoubleValue (offset + Prss));
  wifiPhy.Set ("CcaMode1Threshold", DoubleValue (0.0));
  /***************************************************************************/

  /** wifi channel **/
  YansWifiChannelHelper wifiChannel;
  wifiChannel.SetPropagationDelay ("ns3::ConstantSpeedPropagationDelayModel");
  wifiChannel.AddPropagationLoss ("ns3::FriisPropagationLossModel");
  // create wifi channel
  Ptr<YansWifiChannel> wifiChannelPtr = wifiChannel.Create ();
  wifiPhy.SetChannel (wifiChannelPtr);

  /** MAC layer **/
  // Add a non-QoS upper MAC, and disable rate control
  NqosWifiMacHelper wifiMac = NqosWifiMacHelper::Default ();
  wifi.SetRemoteStationManager ("ns3::ConstantRateWifiManager", "DataMode",
                                StringValue (phyMode), "ControlMode",
                                StringValue (phyMode));
  // Set it to ad-hoc mode
  wifiMac.SetType ("ns3::AdhocWifiMac");

  /** install PHY + MAC **/
  NetDeviceContainer sensordevices = wifi.Install (wifiPhy, wifiMac, SensorNodes);
  NetDeviceContainer sinkdevices = wifi.Install (wifiPhy, wifiMac, SinkNodes);

  //Need to make a random placement of the nodes. 
  /** mobility **/
//Sensors
   srand ( time(NULL) );
  for (int i = 0; i < 100; i++){
     int x = rand() % 100;
     int y = rand() % 100;
     MobilityHelper mobility;
     Ptr<ListPositionAllocator> positionAlloc = CreateObject<ListPositionAllocator> ();
     positionAlloc->Add (Vector (x, y, 0.0));
     positionAlloc->Add (Vector (x, y, 0.0));
     mobility.SetPositionAllocator (positionAlloc);
     mobility.SetMobilityModel ("ns3::ConstantPositionMobilityModel");
     mobility.Install (c.Get (i));

     //Ptr<WSNSensorNode> SensorNode = c.Get (i);
     Ptr<WSNSensorNode> SensorNode = DynamicCast <WSNSensorNode> (c.Get(i));
     SensorNode->x = x;
     SensorNode->y = y;
     NS_LOG_UNCOND ("Sensor NodeID:"  << SensorNode->NodeID << " X:"<<x<<" Y:"<<y);

  }
//Sinks
  for (int i = 0; i < 4; i++){
     int x = rand() % 100;
     int y = rand() % 100;
     MobilityHelper mobility;
     Ptr<ListPositionAllocator> positionAlloc = CreateObject<ListPositionAllocator> ();
     positionAlloc->Add (Vector (x, y, 0.0));
     positionAlloc->Add (Vector (x, y, 0.0));
     mobility.SetPositionAllocator (positionAlloc);
     mobility.SetMobilityModel ("ns3::ConstantPositionMobilityModel");
     mobility.Install (SinkNodes.Get (i));

     //Ptr<WSNSensorNode> SensorNode = c.Get (i);
     Ptr<WSNSinkNode> SinkNode = DynamicCast <WSNSinkNode> (SinkNodes.Get(i));
     SinkNode->x = x;
     SinkNode->y = y;

     NS_LOG_UNCOND ("Sink NodeID:"  << SinkNode->NodeID << " X:"<<x<<" Y:"<<y);

  }



  /*MobilityHelper mobility;
  Ptr<ListPositionAllocator> positionAlloc = CreateObject<ListPositionAllocator> ();
  positionAlloc->Add (Vector (0.0, 0.0, 0.0));
  positionAlloc->Add (Vector (2 * distanceToRx, 0.0, 0.0));
  mobility.SetPositionAllocator (positionAlloc);
  mobility.SetMobilityModel ("ns3::ConstantPositionMobilityModel");
  mobility.Install (c);*/

  /** Energy Model **/
  /***************************************************************************/
  /* energy source */
  BasicEnergySourceHelper basicSourceHelper;
  // configure energy source
  basicSourceHelper.Set ("BasicEnergySourceInitialEnergyJ", DoubleValue (.5)); //energy source intial joules 
  basicSourceHelper.Set ("BasicEnergySupplyVoltageV", DoubleValue (3.0)); //Assume 1.5V of energy per AA battery. We will use two
  // install source
  EnergySourceContainer sources = basicSourceHelper.Install (c);
  /* device energy model */
  WifiRadioEnergyModelHelper radioEnergyHelper;
  // configure radio energy model
  radioEnergyHelper.Set ("TxCurrentA", DoubleValue (0.017));//TX -17dbm:10ma, -3dbm:13ma, 3dbm:17ma
  radioEnergyHelper.Set ("RxCurrentA", DoubleValue (0.016));//Recieve Mode
  radioEnergyHelper.Set ("IdleCurrentA", DoubleValue (0.008)); //Idle Mode
  radioEnergyHelper.Set ("CcaBusyCurrentA", DoubleValue (0.008)); //same as idle
  radioEnergyHelper.Set ("SwitchingCurrentA", DoubleValue (0.008)); //same as idle
  // install device model
  DeviceEnergyModelContainer deviceModels = radioEnergyHelper.Install (sensordevices, sources);
  //DeviceEnergyModelContainer deviceModels = radioEnergyHelper.Install (sinkdevices, sources);
  /***************************************************************************/

  /** Internet stack **/
  InternetStackHelper internet;
  internet.Install (SensorNodes);
  internet.Install (SinkNodes);


  Ipv4AddressHelper ipv4;
  NS_LOG_INFO ("Assign IP Addresses.");
  ipv4.SetBase ("10.1.1.0", "255.255.255.0");
  Ipv4InterfaceContainer sensori = ipv4.Assign (sensordevices);
  Ipv4InterfaceContainer sinki = ipv4.Assign (sinkdevices);


  ///NEED TO LOOP HERE TO MAKE CONNECTS ///////////////////////////

  
//load one hop neighbors as a link to each 

  //This may be taken out and just use the range and hello(once i figure out how ranges work.)

  for (int i = 0; i < 104; i++){
        loadOneHopNeighbors(SinkNodes, SensorNodes, i);
  }


  /*TypeId tid = TypeId::LookupByName ("ns3::UdpSocketFactory");
  Ptr<Socket> recvSink = Socket::CreateSocket (SensorNodes.Get (1), tid);  // node 1, receiver
  InetSocketAddress local = InetSocketAddress (Ipv4Address::GetAny (), 80); //binds to its own listner port on 80
  recvSink->Bind (local);
  recvSink->SetRecvCallback (MakeCallback (&ReceivePacket));//handles incomming messages.
  InetSocketAddress testrem = InetSocketAddress (Ipv4Address::GetBroadcast (), 80);
  recvSink->SetAllowBroadcast (true);
  recvSink->Connect (testrem);

  Ptr<Socket> source = Socket::CreateSocket (SensorNodes.Get (0), tid);    // node 0, sender
  InetSocketAddress remote = InetSocketAddress (Ipv4Address::GetBroadcast (), 80);
  source->SetAllowBroadcast (true);
  source->Connect (remote);
  InetSocketAddress testlocal = InetSocketAddress (Ipv4Address::GetAny (), 80); //binds to its own listner port on 80
  source->Bind (testlocal);
  source->SetRecvCallback (MakeCallback (&ReceivePacket));//handles incomming messages.*/


  /////////////////////////END OF CONNECT LOOPS////////////////////

  /** connect trace sources **/
  /***************************************************************************/
  // all sources are connected to node 1
  // energy source
  Ptr<BasicEnergySource> basicSourcePtr = DynamicCast<BasicEnergySource> (sources.Get (1));
  //basicSourcePtr->TraceConnectWithoutContext ("RemainingEnergy", MakeCallback (&RemainingEnergy));
  // device energy model
  Ptr<DeviceEnergyModel> basicRadioModelPtr =
    basicSourcePtr->FindDeviceEnergyModels ("ns3::WifiRadioEnergyModel").Get (0);
  NS_ASSERT (basicRadioModelPtr != NULL);
  //basicRadioModelPtr->TraceConnectWithoutContext ("TotalEnergyConsumption", MakeCallback (&TotalEnergy));
  /***************************************************************************/

  for (int i = 0; i < 4; i++){
        Ptr<SinkApplication> c0 = Create<SinkApplication> ();//SinkNodes.Get(i));
        c0->SetStartTime(Seconds(1.0));
        c0->SetStopTime (Seconds(5.0));
        NS_LOG_UNCOND ("Sink NodeID:"  << i);
        SinkNodes.Get(i)->AddApplication(c0);
  }
  
  for (int i = 0; i < 100; i++){
        Ptr<SensorApplication> c0 = Create<SensorApplication> ();//SensorNodes.Get(i));
        c0->SetStartTime(Seconds(1.0));
        c0->SetStopTime (Seconds(5.0));
        SensorNodes.Get(i)->AddApplication(c0);
  }

  


  /** simulation setup **/
  // start traffic
  //Simulator::Schedule (Seconds (startTime), &GenerateTraffic, source, PpacketSize,
  //                     SensorNodes.Get (0), numPackets, interPacketInterval);

  Simulator::Stop (Seconds (300.0));
  Simulator::Run ();
  Simulator::Destroy ();

  return 0;
}




















