// Intersect a square with center at (x,y), orientation theta, and the given side length with the set of rectangles.
// If the square lies outside of all obstacles, return true
bool isValidSquare(double x, double y, double theta, double sideLength, const std::vector <Rectangle> &obstacles) {

    double halfLength = sideLength / 2;
    double upperX, upperY, lowerX, lowerY;

    double blX, blY;
    double tlX, tlY;
    double trX, trY;
    double brX, brY;

    upperX = halfLength;
    upperY = halfLength;
    lowerX = -halfLength;
    lowerY = -halfLength;

    //rotate the square, calculate bottom left X, bottom left Y, top left X, etc...
    blX = rotatedX(lowerX, lowerY, theta, x, y);
    blY = rotatedY(lowerX, lowerY, theta, x, y);
    tlX = rotatedX(lowerX, upperY, theta, x, y);
    tlY = rotatedY(lowerX, upperY, theta, x, y);
    trX = rotatedX(upperX, upperY, theta, x, y);
    trY = rotatedY(upperX, upperY, theta, x, y);
    brX = rotatedX(upperX, lowerY, theta, x, y);
    brY = rotatedY(upperX, lowerY, theta, x, y);

    int rectCount = -1;

    for (Rectangle rect: obstacles) {
        rectCount++;

        // check if square's center is inside rectangle
        if (between(rect.x, rect.x + rect.width, x) && between(rect.y, rect.y + rect.height, y))
            return false;

        // check if left side of robot intersects the rectangle
        else if (checkLineToRect(blX, blY, tlX, tlY, rect))
            return false;

        // check top side
        else if (checkLineToRect(tlX, tlY, trX, trY, rect))
            return false;

        // check bottom side
        else if (checkLineToRect(blX, blY, brX, brY, rect))
            return false;

        // check right side
        else if (checkLineToRect(brX, brY, trX, trY, rect))
            return false;

    }

    return true;
}
//=============================================================================
// Compute corners of rotated box, projection edges and min and max projections
// 0---1  corner numbers
// |   |
// 3---2
//=============================================================================
void Entity::computeRotatedBox()
{
    if(rotatedBoxReady)
        return;
    float projection;

    VECTOR2 rotatedX(cos(spriteData.angle), sin(spriteData.angle));
    VECTOR2 rotatedY(-sin(spriteData.angle), cos(spriteData.angle));

    const VECTOR2 *center = getCenter();
    corners[0] = *center + rotatedX * ((float)edge.left*getScale())  +
                           rotatedY * ((float)edge.top*getScale());
    corners[1] = *center + rotatedX * ((float)edge.right*getScale()) + 
                           rotatedY * ((float)edge.top*getScale());
    corners[2] = *center + rotatedX * ((float)edge.right*getScale()) + 
                           rotatedY * ((float)edge.bottom*getScale());
    corners[3] = *center + rotatedX * ((float)edge.left*getScale())  +
                           rotatedY * ((float)edge.bottom*getScale());

    // corners[0] is used as origin
    // The two edges connected to corners[0] are used as the projection lines
    edge01 = VECTOR2(corners[1].x - corners[0].x, corners[1].y - corners[0].y);
    graphics->Vector2Normalize(&edge01);
    edge03 = VECTOR2(corners[3].x - corners[0].x, corners[3].y - corners[0].y);
    graphics->Vector2Normalize(&edge03);

    // this entities min and max projection onto edges
    projection = graphics->Vector2Dot(&edge01, &corners[0]);
    edge01Min = projection;
    edge01Max = projection;
    // project onto edge01
    projection = graphics->Vector2Dot(&edge01, &corners[1]);
    if (projection < edge01Min)
        edge01Min = projection;
    else if (projection > edge01Max)
        edge01Max = projection;
    // project onto edge03
    projection = graphics->Vector2Dot(&edge03, &corners[0]);
    edge03Min = projection;
    edge03Max = projection;
    projection = graphics->Vector2Dot(&edge03, &corners[3]);
    if (projection < edge03Min)
        edge03Min = projection;
    else if (projection > edge03Max)
        edge03Max = projection;

    rotatedBoxReady = true;
}