/* Copyright Steve Rabin, 2012.
 * All rights reserved worldwide.
 *
 * This software is provided "as is" without express or implied
 * warranties. You may freely copy and compile this source into
 * applications you distribute provided that the copyright text
 * below is included in the resulting source code, for example:
 * "Portions Copyright Steve Rabin, 2012"
 */
#include <vector>
#include <array>
#include <list>
#include <Stdafx.h>

bool Movement::ComputePath( int r, int c, bool newRequest )
{
	m_goal = g_terrain.GetCoordinates( r, c );
	m_movementMode = MOVEMENT_WAYPOINT_LIST;
	// project 2: change this flag to true
	bool useAStar = true;
    bool returnStatus = false;

	if( useAStar )
	{
        ///////////////////////////////////////////////////////////////////////////////////////////////////
        //INSERT YOUR A* CODE HERE
        //1. You should probably make your own A* class.
        //2. You will need to make this function remember the current progress if it preemptively exits.
        //3. Return "true" if the path is complete, otherwise "false".
        ///////////////////////////////////////////////////////////////////////////////////////////////////
        Node* cheapest;

        D3DXVECTOR3 cur;
        float hCost = 0;
        Node* start;
        Node* goal;
        int curR, curC;

        //get current position
        cur = m_owner->GetBody().GetPos();
        //get terrain coordinates
        g_terrain.GetRowColumn(&cur, &curR, &curC);

        if (newRequest)
        {

            int mapNum = g_terrain.GetMapIndex();
            //reset/Clear data for new search
            m_waypointList.clear();

            if (m_Astar.currentMapIndex != mapNum)
            {
                m_Astar.LoadNewGrid();
                //get map size
                m_Astar.currentMapIndex = mapNum;
            }
            else
                m_Astar.ClearGrid();

            start = m_Astar.a_grid[curR][curC];
            goal = m_Astar.a_grid[r][c];
            //calculate the new heuristic cost to be used
            hCost = m_Astar.CalcHeuristicCost(GetHeuristicCalc(), start, goal);

            //check for straight line op
            if (m_straightline)
            {
                if (m_Astar.StraightLineOp(start, goal))
                {
                    m_waypointList.push_back(g_terrain.GetCoordinates(curR, curC));
                    m_waypointList.push_back(g_terrain.GetCoordinates(r, c));
                    return true;
                }
            }

            //push start node onto open list
            m_Astar.a_openList.push_back(m_Astar.a_grid[curR][curC]);
        }

        goal = m_Astar.a_grid[r][c];

        while (!m_Astar.a_openList.empty())
        {
            //find cheapest node
            cheapest = m_Astar.PopCheapest();

            m_Astar.GetNeighbors(cheapest);

            //if node is goal node, return found
            if (cheapest->x == r && cheapest->y == c)
            {
                returnStatus = true;
                break;
            }

            Node* child = nullptr;
            float squareRoot = sqrtf(2.0f);

            for (int i = 0; i < 8; ++i)
            {
                child = cheapest->neighbors[i];
                if (child != nullptr)
                {
                    float givenC = 0;
                    int mod = 0;
                    if (i < 4)
                    {
                        givenC = cheapest->givenCost + (i % 2) + squareRoot * ((i + 1) % 2);
                    }
                    else
                    {
                        mod = 1;
                        givenC = cheapest->givenCost + ((i + mod) % 2) + squareRoot * ((i + 1 + mod) % 2);
                    }

                    hCost = m_Astar.CalcHeuristicCost(GetHeuristicCalc(), child, goal);

                    //compute cost: f(x) = g(x) + h(x)
                    //if child isnt on Open or Closed, put it on Open list
                    if (child->status == ListStatus::OnOpenList)
                    {
                        //check if new cost is cheaper
                        if (givenC < child->givenCost)
                        {
                            child->givenCost = givenC;
                            child->parent = cheapest;
                        }
                    }
                    else if(child->status == ListStatus::Free)
                    {
                        child->givenCost = givenC;
                        child->totalCost = givenC + (hCost * m_heuristicWeight);
                        child->parent = cheapest;
                        child->status = ListStatus::OnOpenList;
                        m_Astar.a_openList.push_back(child);
                        if (!g_terrain.IsWall(cheapest->x, cheapest->y))
                            g_terrain.SetColor(child->x, child->y, DEBUG_COLOR_CYAN);
                    }
                }
            }

            //Place parent node on Closed List
            cheapest->status = ListStatus::OnClosedList;
            if(!g_terrain.IsWall(cheapest->x, cheapest->y))
                g_terrain.SetColor(cheapest->x, cheapest->y, DEBUG_COLOR_YELLOW);

            if (m_singleStep)
                return false; //return working
        }//End of while

        if (returnStatus)
        {
            if (m_rubberband)
                m_Astar.RubberBandPath(cheapest);

            if (m_smooth)
            {
                Node* current = cheapest;
                std::vector<D3DXVECTOR3> pointList;

                while (current != nullptr)
                {
                    m_waypointList.push_front(g_terrain.GetCoordinates(current->x, current->y));
                    pointList.push_back(g_terrain.GetCoordinates(current->x, current->y));
                    current = current->parent;

                }

                CatmullSmoothing(pointList);
            }
            else
            {
                Node* current = cheapest;
                while (current != nullptr)
                {
                    m_waypointList.push_front(g_terrain.GetCoordinates(current->x, current->y));
                    current = current->parent;

                }
            }
        }
        else if (m_Astar.a_openList.empty())
        {
            m_waypointList.clear();
            m_waypointList.push_back(g_terrain.GetCoordinates(curR, curC));
            return true;
        }

        return returnStatus;
	}
	else
	{
		//Randomly meander toward goal (might get stuck at wall)
		int curR, curC;
		D3DXVECTOR3 cur = m_owner->GetBody().GetPos();
		g_terrain.GetRowColumn( &cur, &curR, &curC );

		m_waypointList.clear();
		m_waypointList.push_back( cur );

		int countdown = 100;
		while( curR != r || curC != c )
		{
			if( countdown-- < 0 ) { break; }

			if( curC == c || (curR != r && rand()%2 == 0) )
			{	//Go in row direction
				int last = curR;
				if( curR < r ) { curR++; }
				else { curR--; }

				if( g_terrain.IsWall( curR, curC ) )
				{
					curR = last;
					continue;
				}
			}
			else
			{	//Go in column direction
				int last = curC;
				if( curC < c ) { curC++; }
				else { curC--; }

				if( g_terrain.IsWall( curR, curC ) )
				{
					curC = last;
					continue;
				}
			}

			D3DXVECTOR3 spot = g_terrain.GetCoordinates( curR, curC );
			m_waypointList.push_back( spot );
			g_terrain.SetColor( curR, curC, DEBUG_COLOR_YELLOW );
		}
		return true;
	}
}

// POP CHEAPEST
Node* A_Star::PopCheapest()
{
    Node* cheapest = a_openList.front();

    if (a_openList.size() == 1)
    {
        Node* temp = a_openList.back();
        a_openList.pop_back();
        return temp;
    }

    int index = 0;
    int counter = 0;

    for (std::vector<Node*>::iterator it = a_openList.begin(); it != a_openList.end(); ++it)
    {
        if ((*it)->totalCost < cheapest->totalCost)
        {
            cheapest = *it;
            index = counter;
        }
        ++counter;
    }

    //swap end node with cheapest and pop back()
    Node* temp = a_openList.back();
    a_openList.back() = cheapest;
    //eapest = temp;
    a_openList[index] = temp;
    temp = a_openList.back();
    temp->status = ListStatus::OnClosedList;
    a_openList.pop_back();
    return temp;
}

// CALC HEURISTIC COSTS
float A_Star::CalcHeuristicCost(int method, Node* start, Node* goal)
{
    float result = 0;
    float xDiff;
    float yDiff;

    switch (method)
    {
        case 0: //euclid
            xDiff = abs(goal->x - start->x);
            yDiff = abs(goal->y - start->y);
            result = sqrt((xDiff * xDiff) + (yDiff * yDiff));
            break;
        case 1: //octile
            xDiff = abs(goal->x - start->x);
            yDiff = abs(goal->y - start->y);
            result = (min(xDiff, yDiff) * sqrt(2)) + (max(xDiff, yDiff) - min(xDiff, yDiff));
            break;
        case 2: //cheby
            result = max(abs(goal->x - start->x), abs(goal->y - start->y));
            break;
        case 3: //manhattan
            result = abs(goal->x - start->x) + abs(goal->y - start->y);
            break;
    }

    return result;
}

// RUBBERBAND PATH
void A_Star::RubberBandPath(Node * goalNode)
{
    Node* firstNode = goalNode; //should be the goal node at this point
    Node* secondNode = firstNode->parent;
    Node* thirdNode = firstNode->parent->parent;
    int maxX, maxY, minX, minY;

    if (secondNode == nullptr)
        return;

    while (thirdNode != nullptr)
    {
        //set bounds for x
        maxX = max(firstNode->x, thirdNode->x);
        minX = min(firstNode->x, thirdNode->x);

        //set bounds for y
        maxY = max(firstNode->y, thirdNode->y);
        minY = min(firstNode->y, thirdNode->y);

        bool abort = false;

        //test for walls to band around
        for (int i = minX; i <= maxX; ++i)
        {
            for (int j = minY; j <= maxY; ++j)
            {
                if (g_terrain.IsWall(i, j))
                {
                    abort = true;
                    break;
                }
                else
                {
                    firstNode->parent = thirdNode;
                }
            }

            if (abort)
                break;
        }

        //now check for new nodes to evaluate
        firstNode = thirdNode;
        secondNode = firstNode->parent;
        thirdNode = firstNode->parent->parent;
    }
}

// ROM SPLINING / SMOOTHING
void Movement::CatmullSmoothing(std::vector<D3DXVECTOR3> pointList)
{
    std::vector<D3DXVECTOR3> points = pointList;
    unsigned pathSize = 0;
    unsigned psize = points.size();
    if (psize == 1)
    {
        m_waypointList.push_back(points[0]);
        return;
    }

    std::vector<D3DXVECTOR3> newPoints;
    if (m_rubberband)
    {
        unsigned pos = 0;

        for (unsigned i = 0; i < psize - 1; i++, pos++)
        {
            int r, c;
            D3DXVECTOR3 v1 = points[i];
            D3DXVECTOR3 v2 = points[i + 1];
            float distance = GetDistance(v1, v2);

            if (distance > 1.49f)
            {
                int add = (int)(distance / 1.5) + 1;
                D3DXVECTOR3 step = (v2 - v1) / (float)add;
                for (int j = 0; j < add; j++)
                {
                    newPoints.push_back(v1 + step * (float)(j));
                }
            }
            else
                newPoints.push_back(v1);
        }

        newPoints.push_back(points[psize - 1]);
    }

    if (newPoints.size() < psize)
        newPoints = points;

    for (unsigned i = 0; i < newPoints.size() - 1; i++)
    {
        D3DXVECTOR3 p2 = newPoints[i];
        D3DXVECTOR3 p3 = newPoints[i + 1];

        D3DXVECTOR3 p1;
        if (i == 0)
            p1 = p2;
        else
            p1 = newPoints[i - 1];

        D3DXVECTOR3 p4;
        if (i == newPoints.size() - 2)
            p4 = p3;
        else
            p4 = newPoints[i + 2];
        m_waypointList.push_front(p2);
        for (int j = 1; j < 4; j++)
        {
            D3DXVECTOR3 out;
            D3DXVec3CatmullRom(&out, &p1, &p2, &p3, &p4, 0.25f * j);
            m_waypointList.push_front(out);
        }
    }
}

// DISTANCE FUNCTION
float Movement::GetDistance(D3DXVECTOR3 c1, D3DXVECTOR3 c2)
{
    //(Dx*Dx+Dy*Dy+Dz*Dz)^.5
    float dx = c2.x - c1.x;
    float dy = c2.y - c1.y;
    float dz = c2.z - c1.z;

    return sqrt((float)(dx * dx + dy * dy + dz * dz));
}

// STRAIGHT LINE OPTIMIZATION
bool A_Star::StraightLineOp(Node * startNode, Node * goalNode)
{
    int maxX, maxY, minX, minY;

    //set bounds for x
    if (startNode->x > goalNode->x)
    {
        maxX = startNode->x;
        minX = goalNode->x;
    }
    else
    {
        maxX = goalNode->x;
        minX = startNode->x;
    }
    //set bounds for y
    if (startNode->y > goalNode->y)
    {
        maxY = startNode->y;
        minY = goalNode->y;
    }
    else
    {
        maxY = goalNode->y;
        minY = startNode->y;
    }

    bool abort = false;

    //test for walls between start and goal
    for (int i = minX; i <= maxX; ++i)
    {
        for (int j = minY; j <= maxY; ++j)
        {
            if (g_terrain.IsWall(i, j))
            {
                return false;
            }
        }
    }

    return true;
}

// LOAD NEW GRID
void A_Star::LoadNewGrid()
{
    int currentMapSize = g_terrain.GetWidth();
    a_openList.clear();

    for (int i = 0; i < currentMapSize; ++i)
    {
        for (int j = 0; j < currentMapSize; ++j)
        {
            if (g_terrain.IsWall(i, j))
            {
                a_grid[i][j] = nullptr;
                continue;
            }

            a_grid[i][j] = new Node();
            a_grid[i][j]->x = i;
            a_grid[i][j]->y = j;
            g_terrain.SetColor(i, j, DEBUG_COLOR_WHITE);
        }
    }
}

void A_Star::ClearGrid()
{
    a_openList.clear();

    for (int i = 0; i < g_terrain.GetWidth(); ++i)
    {
        for (int j = 0; j < g_terrain.GetWidth(); ++j)
        {
            if (a_grid[i][j] != nullptr)
            {
                a_grid[i][j]->parent = nullptr;
                a_grid[i][j]->givenCost = 0;
                a_grid[i][j]->totalCost = 0;
                a_grid[i][j]->status = ListStatus::Free;

                if (!g_terrain.IsWall(i, j))
                    g_terrain.SetColor(i, j, DEBUG_COLOR_WHITE);
            }
        }
    }
}

// GET NEIGHBORS
void A_Star::GetNeighbors(Node* node)
{
    int curX = node->x;
    int curY = node->y;

    node->neighbors[0] = a_grid[curX - 1][curY + 1];
    node->neighbors[1] = a_grid[curX][curY + 1];
    node->neighbors[2] = a_grid[curX + 1][curY + 1];

    node->neighbors[3] = a_grid[curX - 1][curY];
    node->neighbors[4] = a_grid[curX + 1][curY];

    node->neighbors[5] = a_grid[curX - 1][curY - 1];
    node->neighbors[6] = a_grid[curX][curY - 1];
    node->neighbors[7] = a_grid[curX + 1][curY - 1];

    //bounds check x
    if (node->x == 0)
    {
        node->neighbors[0] = nullptr;
        node->neighbors[3] = nullptr;
        node->neighbors[5] = nullptr;
    }
    else if (node->x == g_terrain.GetWidth() - 1)
    {
        node->neighbors[2] = nullptr;
        node->neighbors[4] = nullptr;
        node->neighbors[7] = nullptr;
    }

    //bounds check y
    if (node->y == 0)
    {
        node->neighbors[5] = nullptr;
        node->neighbors[6] = nullptr;
        node->neighbors[7] = nullptr;
    }
    else if (node->y == g_terrain.GetWidth() - 1)
    {
        node->neighbors[0] = nullptr;
        node->neighbors[1] = nullptr;
        node->neighbors[2] = nullptr;
    }

    //wall check
    for (int i = 0; i < 8; ++i)
    {
        /*  0 / 1 / 2  //
        //  3 / n / 4  //
        //  5 / 6 / 7  */
        if (node->neighbors[i] != nullptr) //if this is an actual node
        {
            if (g_terrain.IsWall(node->neighbors[i]->x, node->neighbors[i]->y))
            {
                node->neighbors[i] = nullptr; //its a wall, we use nullptrs for your kind
                continue;
            }

            // CHECK YOUR CORNERS MATE //
            //now check for corner squares
            //check top left
            if (i == 0)
            {
                if (node->neighbors[1] == nullptr)
                {
                    node->neighbors[0] = nullptr;
                }

                if(node->neighbors[3] == nullptr)
                {
                    node->neighbors[0] = nullptr;
                }
            }
            //top right
            else if (i == 2)
            {
                if (node->neighbors[1] == nullptr)
                {
                    node->neighbors[2] = nullptr;
                }

                if (node->neighbors[4] == nullptr)
                {
                    node->neighbors[2] = nullptr;
                }
            }
            //bottom left
            else if (i == 5)
            {
                if (node->neighbors[3] == nullptr)
                {
                    node->neighbors[5] = nullptr;
                }

                if (node->neighbors[6] == nullptr)
                {
                    node->neighbors[5] = nullptr;
                }
            }
            //bottom right
            else if (i == 7)
            {
                if (node->neighbors[4] == nullptr)
                {
                    node->neighbors[7] = nullptr;
                }

                if (node->neighbors[6] == nullptr)
                {
                    node->neighbors[7] = nullptr;
                }
            }
        }
    }
}