#ifndef __DRONE_H
#define __DRONE_H

#include "common.h"
#include "environmentConst.h"

#define INTACT 1
#define BROKEN 0

#define DRONE_MAX_NUM 729

#define _mass 0.9
#define _collisionR 0.2
#define _iniCommRangeOptimal 50
#define _iniCommRangeMax 100
#define _iniSeparTooClose 20
#define _maxThrust 100
#define _maxVel 50

#define blockLen 50 //detection range 50m

#define xBlockNum ENV_LENGTH/blockLen
#define yBlockNum ENV_WIDTH/blockLen
#define zBlockNum ENV_HEIGHT/blockLen

// block status
#define UNCHECKED 0
#define CHECKED_NO_TARG 1
#define CHECKED_FOUND_TARG 2

#define MAX_RELAY_NUM 5
#define TARG_RELAY_NUM 99999 //relay num if found target

typedef struct MsgNode
{
    vector3d blockIdx;
    int status;
    int relayCtr;
    MsgNode *next;

    MsgNode();
    MsgNode(vector3d index, int status, int relayNum);
    ~MsgNode();
}MsgNode;

typedef class Drone
{
    friend class Environment;
    private:
        /*****Internal parameters****/
        int droneIndex;

        /****entity para****/
        vector3d Pos;
        vector3d Vel;
        vector3d Acc;
        vector3d Force;
        /**entity para end**/

        double battery;
        double collisionR;
        double commRangeOptimal;
        double commRangeMax;
        double maxThrust;
        double totalDis;
        double integrity;

        int peerNumN;
        int peerNumF;

        double maxVel;
        struct
        {
            int idx; //peer drone index
            int dis; // distance
            vector3d relPos; //relative pos
        }peerList[DRONE_MAX_NUM + 2]; //[0] is pivot, reserved for Partition

        int peerListMid; //point to the pivot,default = 1
        int peerListEnd; //point to the last,default = 1

        int peerTooClosePtr;//point to the second pivot,default = 0

        double mass;

        vector3d thrust;

        bool targetFound;
        vector3d targetIdx; //if target found, this stores target index

        /*****Internal parameters end****/

        int map[xBlockNum][yBlockNum][zBlockNum]; //built-in sectorized map
        
        MsgNode receiveHead;//linklist to buffer received info
        MsgNode transmittHead;//linklist to buffer info to be transmitted
        



        ///////
        bool inBound();//check if current pos coordinate is inbound
        bool idxInbound(vector3d idx); //check if index is inbound
        bool getIdx(int &xIdx, int &yIdx, int &zIdx);  //get current block index, return false if exceed outer block bound

        
        void addTransmitt(vector3d idx, int status,int relayNum); // 1.update transmitt list if updateSelfMap(), 2.broadcast target if target is found
        

        
        //void clearMsgList(MsgNode *head);

        int getMap(vector3d idx); //get map status via index
        int setMap(vector3d idx, int status);  //set map status

        vector3d currentBlockIndex(); //get current block index
        /***********************************/

        void sortPeerList();
        void updateSelfMap(); //update map based on self location
        void updateReceive(); // update map based on info received
        void updateTransmitt(); // update transmitt linklist. CALLED BY droneBroadcase() from environment, basically relayCtr--

        /***********************************/
    public:
        Drone(int droneIndex, vector3d dronePos, vector3d droneVel, double battery);
        ~Drone();

        void addMsg(vector3d index, int status, int relayNum); //for peer to leave msg, add node to linklist

        void do_execute();

        void printPara(); //print basic parameters
        
}Drone;

#endif