// states vectors
float debris[41][4];
float myPos[3];
float targetPos[3];
float myState[12];
float otherState[12];
float circle_target[8][2];

// vector between
float vb[3];
float vp[3];

// point to rende and point to go
float pointToRende[3];
float pointToGo[3];

//set force
float force[3];
float forceStop[3];

// count
int c;
int d;

//states
bool canGo;
bool found;

// distances
float dis;
float oDis;

// end and start position
int target[3];
float startingPos[3];

// gridsize and debris for pathfinding
int gridsize;
int intDebris[41][4];

//hooking
float hookPos[3];
float targetRattHook[3];
float targetRattHookBack[3];
float dot;
float attitude[3];
float iniAttitude[3];
float angle;
int hookPhaseCount;
bool OverlapedHookCenter;
float ReSetRate[3];



class Node{

public:

  Node* next;
  Node* last;
  Node* parent; // keep track of the parent, aka which node we generated from
  float H; // euristic distance between end node
  int G; // distance from starting node, assuming a cost NOTE: can be changed to float (change casts too)
  float F; // G + H (+ dmg)
  float distanceToDebris;
  int position[3]; // x y z
  int dmg; //dmg penality for hitting debris objects

  Node() {};
  Node(  int _position[]){
      position[0] = _position[0];
      position[1] = _position[1];
      position[2] = _position[2];
      H = 0;
      F = 0;
      G = 0;
      next = NULL;
      last = NULL;
      parent = NULL;
      dmg = 0;
      distanceToDebris = 0;
  };
  Node(int x, int y, int z, Node* _parent = NULL) { // the constructor we actually use
      position[0] = x;
      position[1] = y;
      position[2] = z;
      H = 0;
      F = 0;
      G = 0;
      dmg = 0;
      distanceToDebris = 0;
      next = NULL;
      last = NULL;
      parent = _parent;
  }


  void calculateG(int prevG, int gaddvalue) {
    G = prevG + gaddvalue;
  }

  void calculateF(int prevG, int gaddvalue, int target[], int debris[][4]) {
    //this->analysisDmg(debris); we dont use analysis dmg for now (see below)
    G = prevG + gaddvalue;
    H = calculateDistance(target[0], target[1], target[2]);
    F = G + H; // + dmg; (not used)
  }
  float calculateDistance(int x, int y, int z = 0) {
      return sqrt(powf((position[0]-x),2)+powf((position[1]-y),2)+powf((position[2]-z),2));
  }

    // For now we consider debris as obstacles and we dont calculate a penality
    /*
    void analysisDmg(int debris[][4]) { we
    dmg = 0;
    for(int i = 0; i < 16; i++) { // i < 11
       // if(i < 4) {
            distanceToDebris = calculateDistance(debris[i][0], debris[i][1], debris[i][2]);
            if(distanceToDebris < (11+debris[i][3]+1))
                dmg += 20;
     }
       else if(i > 3 && i < 8) {
            distanceToDebris = calculateDistance(debris[i][0], debris[i][1], debris[i][2]);
            if(distanceToDebris < (11+debris[i][3]+2))
                dmg += 500;
    }
    else {
            distanceToDebris = sqrt(powf((position[0]-debris[i][0]),2)+powf((position[1]-debris[i][1]),2));
            if(distanceToDebris < (11+debris[i][3]+3))
                dmg += 1000;
    }
   }
    } */

  float xToFloat() {
      return (float) position[0] / 100;
  }
  float yToFloat() {
      return (float) position[1] / 100;
  }
  float zToFloat() {
      return (float) position[2] / 100;
  }
  void print() {
     // std::cout << position[0] << "\t" << position[1] << std::endl; // cout used to debug with gcc
  }

};


class Path {

   private:

   Node* openList;
   Node* closedList;
   Node* lastOpenList;
   Node* lastClosedList;
   Node* finalPath;
   Node* lastFinalPath;

   public:

   Path(){

       // create open list
       openList = new Node();
       openList->next = NULL;
       openList->last = NULL;

       // create closed list
       closedList = new Node();
       closedList->next = NULL;
       closedList->last = NULL;

       //create the list containing the actual path
       finalPath = new Node();
       finalPath->next = NULL;
       finalPath->last = NULL;

   }



   ~Path(){
       clearList();
       delete openList;
       openList = NULL;

       clearClosedList();
       delete closedList;
       closedList = NULL;
   }
   // modify this to debug
    void printClosedList() {
       Node* currentNode = closedList->next;
       while(currentNode != NULL) {
           currentNode->print();
        //   std::cout << std::endl;
           currentNode = currentNode->next;
       }
   }
   void printOpenList() {
       Node* currentNode = openList->next;
       while(currentNode != NULL) {
           currentNode->print();
         //  std::cout << std::endl;
           currentNode = currentNode->next;
       }
   }
   void printFinalPath() {
       Node* currentNode = lastFinalPath;
       while(currentNode != finalPath) {
           currentNode->print();
          // std::cout << std::endl;
           currentNode = currentNode->last;
       }
   }

   Node* returnOpenLast() {
       return lastOpenList;
   }
   Node* returnClosedLast() {
       return lastClosedList;
   }
   Node* returnFPath() {
       return finalPath->next;
   }
   // determine if a position is inside the radius of debris+sphere (add 3 to mitigate collisions)
   bool isInRadius(int x, int y, int z, int debrisX, int debrisY, int debrisZ,  int r) {
	   float distance = sqrt(powf((x-debrisX),2)+powf((y-debrisY),2)+powf((z-debrisZ),2));
       if(distance < (11+r+3))
               return true;
        return false;
    }


   bool isValid(int x, int y, int z, int debris[][4]) {
      //check if the Node is inside a closed list
      if(this->isInClosedList(x, y, z))
          return false;
      //prevent the sphere to go OFB (Out Of Bounds)
	  if(x <= -64+11 || x >= 64-11)
		  return false;
	  if(y <= -80+11 || y >= 80-11)
		  return false;
	  if(z <= -64+11 || z >= 64-11)
	      return false;
	  //check if the Node can cause a collision
	  for(int i = 0; i < 41; i++) {
		if(i > 32) {
            if(isInRadius(x, y, z, debris[i][0], debris[i][1], debris[i][2], debris[i][3]))
				return false;
		}
		if(i < 32 && i > 16) {
		    if(isInRadius(x, y, z, debris[i][0], debris[i][1], debris[i][2], debris[i][3]))
				return false;
	   }
	   else {
            if(isInRadius(x, y, z, debris[i][0], debris[i][1], debris[i][2], debris[i][3]))
				return false;
		}
	  }
	  return true;
  }
   void optimizePath( Node* currentNodeStartToOptimize)
   {
      // This functions lets us to optimize the path by deleting nodes when we dont change angle
      // Needs to be rewritten, only works for 2D
      if(currentNodeStartToOptimize == NULL || currentNodeStartToOptimize->next == NULL || currentNodeStartToOptimize->next->next == NULL)
      {
          return;
      }

      if(currentNodeStartToOptimize->last == NULL)
      {
          return;
      }

       Node* currentNode = currentNodeStartToOptimize;

       float x1 = 0;
       float x2 = 0;
       float z1 = 0;
       float z2 = 0;
       float x3 = 0;
       float y1 = 0;
       float y2 = 0;
       float y3 = 0;
       float m1 = 0;
       float m2 = 0;
       bool needToDelete = false;
       bool changedZ = false;


       x1 = currentNode->position[0];
       y1 = currentNode->position[1];
       z1 = currentNode->position[2];

       currentNode = currentNode->next;

       x2 = currentNode->position[0];
       y2 = currentNode->position[1];
       z2 = currentNode->position[2];
       if(z1 != z2) {
            changedZ = true;
       }

       if(x1 == x2)
       {
           needToDelete = true;
       }
       else
       {
           m1 = (y2 - y1) / (x2 - x1);
       }


       currentNode = currentNode->next;

       x3 = currentNode->position[0];
       y3 = currentNode->position[1];

       if(x2 != x3)
       {
           needToDelete = false;
           m2 = (y3 - y2) / (x3 - x2);
       }
       else
       {
           m2 = m1 + 1;
       }


       if((m1 == m2 || needToDelete) && changedZ)
       {
           Node* temp2 = currentNode;
           currentNode = currentNode->last;
           Node* temp1 = currentNode->last;
           temp1->next = temp2;
           temp2->last = temp1;
           delete currentNode;
           optimizePath(temp1);
       }
       else
       {
           optimizePath(currentNode);
       }

   }
  void addToOpenList(Node *node){
       //exit if memory cannot be allocated
       if(openList->next == NULL)
       {
           openList->next = node;
           node->last = openList;
           node->next = NULL;
       }
       else
       {
           Node* currentNode = openList->next;
           while(currentNode->next != NULL)
           {
               currentNode = currentNode->next;
           }
           currentNode->next = node;
           node->last = currentNode;
           node->next = NULL;
       }
       lastOpenList = node;
   }

   void addToClosedList(Node *node)
   {
       if(closedList->next == NULL)
       {
           closedList->next = node;
           node->last = closedList;
           node->next = NULL;
       }
       else
       {
           Node* currentNode = closedList->next;
           while(currentNode->next != NULL)
           {
               currentNode = currentNode->next;
           }
           currentNode->next = node;
           node->last = currentNode;
           node->next = NULL;
       }
       lastClosedList = node;
   }

   void clearClosedList()
   {
       Node* currentNode = closedList->next;
       while(currentNode != NULL)
       {
           Node* temp = currentNode->next;
           delete currentNode;
           currentNode = temp;
       }
       closedList->next = NULL;
       lastClosedList = NULL;
   }

   void clearFinalPath()
   {
       Node* currentNode = finalPath->next;
       while(currentNode != NULL)
       {
           Node* temp = currentNode->next;
           delete currentNode;
           currentNode = temp;
       }
       finalPath->next = NULL;
       lastFinalPath = NULL;
   }
     void createNeighbors(Node* node, int intDebris[][4], int gridsize, int target[]) {
     int x, y, z, g, costr, costd;
     Node* temp = NULL;
     x = node->position[0];
     y = node->position[1];
     z = node->position[2];
     g = node->G;
     costr = gridsize; //normal cost
     costd = (int) costr * 1.4; //diagonal cost
     //costr = 10;
     //costd = 14;
     // check and create nodes above and below the node
     if(isValid(x, y, z - gridsize, intDebris)) {
             temp = this->findInOpenList(x - gridsize, y - gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x - gridsize, y - gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costr, target, intDebris);
        }
    if(isValid(x , y , z + gridsize, intDebris)) {
             temp = this->findInOpenList(x - gridsize, y - gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x - gridsize, y - gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costr, target, intDebris);
        }
     // check and/or create nodes for every possible direction, starting from node
     for(int i = 0; i < 3; i++) {
         if(i == 1) {
             z += gridsize;
             costr = costd;
             costd = (int) gridsize * 1.73; // diagonal of a cube

         }
         else if(i == 2) {
             z -= (gridsize*2);
             //costd = 17;
             //costr = 14;
         }
         if(isValid(x, y + gridsize, z, intDebris)) {
             temp = this->findInOpenList(x, y + gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x, y + gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costr, target, intDebris);
            }

         if(isValid(x, y - gridsize, z, intDebris)) {
             temp = this->findInOpenList(x, y - gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x, y - gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costr, target, intDebris);
            }
         if(isValid(x + gridsize, y, z, intDebris)) {
             temp = this->findInOpenList(x + gridsize, y, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x + gridsize, y, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costr, target, intDebris);
            }
         if(isValid(x - gridsize, y, z, intDebris)) {
             temp = this->findInOpenList(x - gridsize, y, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x - gridsize, y, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costr, target, intDebris);
            }
         if(isValid(x - gridsize, y  - gridsize, z, intDebris)) {
             temp = this->findInOpenList(x - gridsize, y - gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x - gridsize, y - gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costd, target, intDebris);
            }
         if(isValid(x + gridsize, y + gridsize, z, intDebris)) {
             temp = this->findInOpenList(x + gridsize, y + gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x + gridsize, y + gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costd, target, intDebris);
            }
         if(isValid(x + gridsize, y - gridsize, z, intDebris)) {
             temp = this->findInOpenList(x + gridsize, y - gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x + gridsize, y - gridsize, z, node));
                 temp = this->returnOpenLast();
             }
            temp->calculateF(g , costd, target, intDebris);

            }
         if(isValid(x - gridsize, y - gridsize, z, intDebris)) {
             temp = this->findInOpenList(x - gridsize, y + gridsize, z);
             if(temp == NULL) {
                 this->addToOpenList(new Node(x - gridsize, y + gridsize, z, node));
                 temp = this->returnOpenLast();
             }
             temp->calculateF(g , costd, target, intDebris);
        }
     }
   }

   void addToFinalPath(Node *node) {
       Node* newNode = new Node();
       newNode->position[0] =  node->position[0];
       newNode->position[1] =  node->position[1];
       newNode->position[2] =  node->position[2];
       if(finalPath->next == NULL) {
           finalPath->next = newNode;
           newNode->last = finalPath;
           newNode->next = NULL;
           lastFinalPath = newNode;
       }
       else
       {
           lastFinalPath->next = newNode;
           newNode->last = lastFinalPath;
           newNode->next = NULL;
           lastFinalPath = newNode;
       }
   }

   void createFinalPath() {
       Node* currentNode = lastClosedList;
       while(currentNode != closedList->next) {
           addToFinalPath(currentNode);
           currentNode = currentNode->parent;
       }
       addToFinalPath(closedList->next);
   }

    Node* chooseLowestScore() {
       Node* currentNode = openList->next;
       Node* bestNode = currentNode;
       while(currentNode->next != NULL) {
           if(currentNode->F == bestNode->F)
            if(currentNode->H < bestNode->H)
               bestNode = currentNode;
            if(currentNode->F < bestNode->F)
               bestNode = currentNode;
           currentNode = currentNode->next;
       }
       return bestNode;
   }
   void deleteBestNode(Node* bestNode) {
        if(bestNode->next == NULL) {
               Node * temp = bestNode->last;
           temp->next = NULL;
           lastOpenList = bestNode->last;
       }

       else {
           bestNode->last->next = bestNode->next;
           bestNode->next->last = bestNode->last;
       }
   }


   bool isInClosedList(int x, int y, int z = 0) {
    Node* currentNode = closedList->next;
    while(currentNode != NULL) {
        if(currentNode->position[0] == x && currentNode->position[1] == y && currentNode->position[2] == z)
            return true;
        currentNode = currentNode->next;
    }
    return false;
   }

   Node* findInOpenList(int x, int y, int z = 0) {
    Node* currentNode = openList->next;
    while(currentNode != NULL) {
        if(currentNode->position[0] == x && currentNode->position[1] == y && currentNode->position[2] == z)
            return currentNode;
        currentNode = currentNode->next;
    }
    return NULL;
   }

   void clearList(){
       Node* currentNode = openList->next;
       while(currentNode != NULL)
       {
           Node* temp = currentNode->next;
           delete currentNode;
           currentNode = temp;
       }
       openList->next = NULL;
   }

   bool readListFromLast(float pointToGo[])

  {
      pointToGo[0] = lastFinalPath->xToFloat();
      pointToGo[1] = lastFinalPath->yToFloat();
      pointToGo[2] = lastFinalPath->zToFloat();
      lastFinalPath = lastFinalPath->last;
      if(lastFinalPath->last == finalPath)
      {
          return true;
      }

  }

};

  Path *path;
  Node *currentNode;


void init(){

    c = 0;
    dis = 0;
    game.getDebris(debris);
    canGo = false;

    // force vectors
    force[0] = 0;
    force[1] = -1;
    force[2] = 0;

    forceStop[0] = 0;
    forceStop[1] = 0;
    forceStop[2] = 0;

    target[0] = 0;
    target[1] = -15;
    target[2] = 20;
    startingPos[0] = 0;
    startingPos[1] = 0.75f;
    startingPos[2] = 0;

    pointToRende[0] = 0.0f;
    pointToRende[1] = -0.15f;
    pointToRende[2] = 0;
    //vb[0] = 0;
    //vb[1] = 0;
    //vb[2] = 0;
    vp[0] = 0;
    vp[1] = -1;
    vp[2] = 0;
    d = 0;
    found = false;
    circle_target[0][0] = 0;
    circle_target[0][1] = -0.2f;
    circle_target[4][0] = 0;
    circle_target[4][1] = 0.2f;
    circle_target[6][0] = -0.2f;
    circle_target[6][1] = 0;
    circle_target[2][0] = 0.2f;
    circle_target[2][1] = 0;

    circle_target[7][0] = -0.15f;
    circle_target[7][1] = -0.12f;
    circle_target[5][0] = -0.12f;
    circle_target[5][1] = 0.15f;
    circle_target[3][0] = 0.12f;
    circle_target[3][1] = 0.15f;
    circle_target[1][0] = 0.15f;
    circle_target[1][1] = -0.12f;

    //hooking
    targetRattHook[0] = 0.00;
    targetRattHook[1] = 0.00;
    targetRattHook[2] = 50*PI/180;
    targetRattHookBack[0] = 0.00;
    targetRattHookBack[1] = 0.00;
    targetRattHookBack[2] = -50*PI/180;
    angle = 0;
    dot = 0;
    hookPhaseCount = 0;
    OverlapedHookCenter = false;
      hookPos[0] = 0.058f;
      hookPos[1] = -0.20f;
      hookPos[2] = 0.2f;

     ReSetRate[0] = 0;
     ReSetRate[1] = 0;
     ReSetRate[2] = 0;

    path = new Path();
    path->addToOpenList(new Node(0, 75, 0));
    currentNode = path->returnOpenLast();

    gridsize = 10;


    // convert axis to int
    for(int i = 0; i < 41; i++)
        for(int j = 0; j < 4; j++) {
            intDebris[i][j] = debris[i][j] * 100;
        }
    do {
         currentNode = path->chooseLowestScore();
         path->deleteBestNode(currentNode);
         path->addToClosedList(currentNode);
         path->createNeighbors(currentNode, intDebris, gridsize, target);
         //path->printOpenList();
         //currentNode->print();
         //DEBUG(("x: %d y: %d \n", currentNode->position[0], currentNode->position[1]));


    } while(currentNode->position[0] != target[0] || currentNode->position[1] != target[1] || currentNode->position[2] != target[2]);
    path->createFinalPath();
    //path->printFinalPath();
    path->optimizePath(path->returnFPath());
    path->readListFromLast(pointToGo);
    mathVecSubtract(vb, pointToGo, startingPos,3);
    oDis = mathVecMagnitude(vb, 3);
}

bool isCloseTo(float &pointA, float &pointB, float tol = 0) {
    if(pointA >= pointB - tol && pointA <= pointB + tol)
        return true;
    return false;
}

bool isBetween(float &pointToCheck, float &pointA, float &pointB) {
    if(pointA < pointB) {
        if(pointToCheck >= pointA && pointToCheck <= pointB)
            return true;
    }
    else {
        if(pointToCheck <= pointA && pointToCheck >= pointB)
            return true;
    }
    return false;
}

void loop(){
    // get my and red states
 api.getMyZRState(myState);
 api.getOtherZRState(otherState);
 // get my and red positions
 for(int i = 0; i < 3; i++)
 {
     targetPos[i] = otherState[i];
     myPos[i] = myState[i];
 }

 // pass debris zone
 float tol = 0.1f;
 if(game.getGamePhase() == 1 || !canGo)
 {
     mathVecSubtract(vb,pointToGo,myPos,3);
     dis = mathVecMagnitude(vb, 3);
     if(isCloseTo(myPos[0], pointToGo[0], tol) && isCloseTo(myPos[1], pointToGo[1], tol) && isCloseTo(myPos[2], pointToGo[2], tol))
     {
        canGo = path->readListFromLast(pointToGo);
        mathVecSubtract(vb,pointToGo,myPos,3);
        oDis = mathVecMagnitude(vb, 3);
        //if(rand) api.setPositionTarget(pointToGo);
        //else api.setVelocityTarget(vb);
        DEBUG(("Reading new node"));

    }
    else
    {
        if(dis > oDis*0.90f) {
            api.setVelocityTarget(vb);
            //rand = true;
            DEBUG(("Setting Velocity Target"));
        }
        else {
            api.setPositionTarget(pointToGo);
            //rand = false;
            DEBUG(("Setting Position Target"));
        }


    }

 }
 if(game.getGamePhase() == 2) {
    mathVecSubtract(vb,targetPos,myPos,3);
    dis = mathVecMagnitude(vb, 3);
    //mathVecNormalize(vb, 3);
    //api.setVelocityTarget(vb);
    //targetPos[]
    api.setPositionTarget(pointToRende);

    while(d == 0){
        for(int i = 0; i < 7; i++){

            if(isBetween(targetPos[0], circle_target[i][0], circle_target[i+1][0]) && isBetween(targetPos[2], circle_target[i][1], circle_target[i+1][1]))
                found = true;

            if(found) {
                i += 2;
                i %= 8;
                pointToRende[0] = circle_target[i][0];
                pointToRende[2] = circle_target[i][1];
                d++;
                break;
            }
        }
    }

    api.setPositionTarget(pointToRende);

    if(game.checkRendezvous())
    {
        game.completeRendezvous();
        if(c == 0)
        {
            delete path;
            c++;
        }
    }
 }
 if(game.getGamePhase() == 3) {

      tol = 0.01;
      if(hookPhaseCount == 0)
      {
          iniAttitude[0] = -otherState[6];
          iniAttitude[1] = -otherState[7];
          iniAttitude[2] = -otherState[8];
      }
       dot = mathVecInner(attitude, iniAttitude, 3);
       angle = acosf(dot)* 180 / PI;
       if(angle <= 10 && OverlapedHookCenter == false && game.getHooked() == false)
       {
           api.setAttRateTarget(targetRattHook);
       }
       else if(angle >= 10 && OverlapedHookCenter == true && game.getHooked() == false)
       {
           api.setAttRateTarget(targetRattHookBack);
       }
        else
       {
           api.setAttRateTarget(ReSetRate);
       }

      mathVecSubtract(vb,targetPos,myPos,3);
      mathVecNormalize(vb, 3);
      api.setAttitudeTarget(vb);
      if(game.getHooked() == false)
       {
          targetPos[1] += 0.4f ;
          api.setPositionTarget(targetPos);

       }


      if(myPos[0] >hookPos[0] - tol && myPos[0] < hookPos[0] + tol   &&    myPos[1] > hookPos[1]- tol && myPos[1] < hookPos[1] + tol && myPos[2] > hookPos[2] - tol && myPos[2] < hookPos[2] + tol)
      {
          OverlapedHookCenter = true;
      }

     vp[0] = otherState[6];
     vp[1] = -otherState[7];


    //if(c++ == 1) {
    //targetPos[1] += 0.35f;
    //api.setPositionTarget(targetPos);
    //d++;
    //vp[0] = otherState[6];
    //vp[1] = -otherState[7];
    //vp[2] = -otherState[8];
    //api.setAttitudeTarget(vp);
 }
}