entity

// Entity uses, takes or wraps around a pathfinder in order to calculate
// its position. entity's location should be a coordinate pair, each element
// a number of pixels.
template <class T>
class entity
{
    pathFinder<T> * pf;
    pair<int, int> pixelCoord; // the current location of the entity in pixels
    pair<int, int> noPixelCoord; // the current element the entity is closest to.
    pair<int, int> pixelDestination;
    pair<int, int> noPixelDestination; // ??
    time_point<system_clock, system_clock::duration> t;
    int pixelsPerSecond = 10;
public:
    entity<T>(pathFinder<T>& p){
        pf = &p;
        pixelCoord.first = p.getBegin().first * p.getLength() - p.getLength() / 2;
        pixelCoord.second = p.getBegin().second * p.getWidth() - p.getWidth() / 2;
        noPixelCoord = p.getBegin();
        t = steady_clock::now();
    }
    int distance(pair<int, int> first, pair<int, int> second){
        int x = first.first, y = first.second;
        int x2 = second.first, y2 = second.second;
        int xdif = x - x2, ydif = y - y2;
        return sqrt(xdif*xdif + ydif*ydif);
    }
    float angle(pair<int, int> first, pair<int, int> second){
        int x = first.first, y = first.second;
        int x2 = second.first, y2 = second.second;
        float numerator = x * x2 + y * y2;
        float denominator = sqrt(x*x + y*y) * sqrt(x2*x2 + y2*y2);
        return (numerator / denominator);
    }
    void update(){
        if (this->findDestination()){
            if (steady_clock::now() - t > milliseconds(1000)){
                int distanc = distance(noPixelCoord, noPixelDestination);
                distanc = (distanc > pixelsPerSecond ? pixelsPerSecond : distanc);
                float angl = angle(noPixelCoord, noPixelDestination);
                noPixelCoord.first += acos(angl) * distanc;
                noPixelCoord.second += asin(angl) * distanc;
                cout << "Updated! ";
            }
        }
    }
    // findDestination gets the closest next square the entity should path to.
    bool findDestination(){
        if (pf->check()){
            if (pf->getEnd() == noPixelCoord) // if the entity has reached the end
                return false;
            // First, check to see if we're one away from the end.
            else if (pf->oneD(noPixelCoord.first + 1, noPixelCoord.second) == pf->oneD(pf->getEnd()) ||
                pf->oneD(noPixelCoord.first - 1, noPixelCoord.second) == pf->oneD(pf->getEnd()) ||
                pf->oneD(noPixelCoord.first, noPixelCoord.second + 1) == pf->oneD(pf->getEnd()) ||
                pf->oneD(noPixelCoord.first, noPixelCoord.second - 1) == pf->oneD(pf->getEnd()))
                noPixelDestination = pf->getEnd();
            else{ // Since we aren't one away from the end, find the closest proper element
                pair<int, int> arr[] = {pair<int, int>(noPixelCoord.first + 1, noPixelCoord.second),
                    pair<int, int>(noPixelCoord.first - 1, noPixelCoord.second),
                    pair<int, int>(noPixelCoord.first, noPixelCoord.second + 1),
                    pair<int, int>(noPixelCoord.first, noPixelCoord.second - 1) };
                list<pair<int, int> > l(arr, arr + sizeof(arr) / sizeof(pair<int, int>));
                l.sort([&](pair<int, int> a, pair<int, int> b){ return pf->operator()(a) < pf->operator()(b); });
                while (pf->operator()(l.front())) // pop all zeros
                    l.pop_front();
                while (pf->operator()(l.front()) == pf->WALL) // pop all walls
                    l.pop_front();
                if (!l.empty()) // get the first lowest number that isn't a wall or a zero
                    noPixelDestination = l.front();
            }

            // Place the pixelDestination in the middle of the square @ nopixeldestination
            pixelDestination.first = noPixelDestination.first * pf->getLength() - pf->getLength() / 2;
            pixelDestination.second = noPixelDestination.second * pf->getWidth() - pf->getWidth() / 2;
        }
        return true;
    }
};