/** CSci-4611 Assignment 2:  Car Soccer
 */

#include "car_soccer.h"
#include "config.h"


// Remember in C++, the .h file list all the functions and member variables that are part of the class!
// Look there first to understand what is part of the CarSoccer class, then look below to see how each
// function is implemented.


CarSoccer::CarSoccer() : GraphicsApp(1024,768, "Car Soccer") {
    // If you are having trouble driving the car with the keybaord, you can set this to true to use
    // the mouse instead.  The mouse controls are based on the postion of the mouse cursor on the window.
    // There is a "dead zone" in the middle of the window, and if you move the mouse up/down or left/right
    // outside of that zone, it is like pushing the up/down and/or left/right keys on the keyboard
    use_mouse_ = false;
        
    // Define a search path for finding data files (images and shaders)
    searchPath_.push_back(".");
    searchPath_.push_back("./data");
    searchPath_.push_back(DATA_DIR_INSTALL);
    searchPath_.push_back(DATA_DIR_BUILD);
}

CarSoccer::~CarSoccer() {
}


void CarSoccer::OnMouseMove(const Point2& pos, const Vector2& delta)
{
    mouse_pos_ = PixelsToNormalizedDeviceCoords(pos);
}

void CarSoccer::OnSpecialKeyDown(int key, int scancode, int modifiers) {
    if (key == GLFW_KEY_SPACE) {
        // Here's where you could call some form of launch_ball();
        ball_.Reset();
    }
}

/// This is a little utility function that is helpful.  It treats the arrow keys like a joystick or D-pad on a game controller
/// and returns the direction you are pressing as a 2D vector, taking into account the fact that you might be holding
/// down more than one key at a time.
Vector2 CarSoccer::joystick_direction() {
    Vector2 dir;

    if (use_mouse_) {
        // threshold defines the size of the "dead zone" in the middle of the screen
        // if the mouse's x,y position falls outside of this, then it is like pushing
        // the corresponding key on the keyboard
        const float threshold = 0.2f;
        dir[0] = 0;
        if (mouse_pos_[0] < -threshold) {
            dir[0] = -1;
        }
        else if (mouse_pos_[0] > threshold) {
            dir[0] = 1;
        }
        dir[1] = 0;
        if (mouse_pos_[1] < -threshold) {
            dir[1] = -1;
        }
        else if (mouse_pos_[1] > threshold) {
            dir[1] = 1;
        }
    }
    else {
        // the default user interface is to use the arrow keys on the keyboard.
        // like a D-pad on a game controller, you can hold more than one key down at a time if you want.
        if (IsKeyDown(GLFW_KEY_LEFT))
            dir[0]--;
        if (IsKeyDown(GLFW_KEY_RIGHT))
            dir[0]++;
        if (IsKeyDown(GLFW_KEY_UP))
            dir[1]++;
        if (IsKeyDown(GLFW_KEY_DOWN))
            dir[1]--;
    }

    return dir;
}

// dt is for "Delta Time", the elapsed time in seconds since the last frame
void CarSoccer::UpdateSimulation(double dt) {
    Vector2 dpad_dir = joystick_direction();
    //std::cout << "D-Pad Direction: " << dpad_dir << std::endl;

    // Here's where you shound do your "simulation", updating the positions of the
    // car and ball based on the elapsed time and checking for collisions.  Filling
    // in this routine is the main part of the assignment.

    /* Ball Routines */
    // ball collision with car
    if ((ball_.position() - car_.position()).Length() <= ball_.radius() + car_.collision_radius()) {
        Vector3 normal = (ball_.position() - car_.position()).ToUnit();
        // collision displacement
        while ((car_.position() - ball_.position()).Length() < ball_.radius() + car_.collision_radius()) {
            ball_.set_position(ball_.position() + normal * 0.1);
        }
        // bounce backwards
        Vector3 carBounce = ball_.velocity() - car_.velocity();
        ball_.set_velocity(0.8 * (car_.velocity() + carBounce - 2 * (carBounce.Dot(normal) * normal)));
    }
    // ball collision with various things
    // there's probably a way to do this with less boilerplate but it works
    if (ball_.position().y() - ball_.radius() <= 0) {
        // ground
        ball_.set_position(Point3(ball_.position().x(), ball_.radius(), ball_.position().z()));
        ball_.set_velocity(0.8 * (ball_.velocity() - 2 * (ball_.velocity().Dot(Vector3(0, 1, 0)) * Vector3(0, 1, 0))));
    }
    if (ball_.position().y() - ball_.radius() >= 35) {
        // ceiling
        ball_.set_position(Point3(ball_.position().x(), 35-ball_.radius(), ball_.position().z()));
        ball_.set_velocity(0.8*(ball_.velocity() - 2 * (ball_.velocity().Dot(Vector3(0, -1, 0)) * Vector3(0, -1, 0))));
    }
    if (ball_.position().z() + ball_.radius() >= 50) {
        // home
        ball_.set_position(Point3(ball_.position().x(), ball_.position().y(),50 - ball_.radius()));
        ball_.set_velocity(0.8 * (ball_.velocity() - 2 * (ball_.velocity().Dot(Vector3(0, 0, -1)) * Vector3(0, 0, -1))));
    }
    if (ball_.position().z() + ball_.radius() <= -50) {
        // away
        ball_.set_position(Point3(ball_.position().x(), ball_.position().y(), ball_.radius() - 50));
        ball_.set_velocity(0.8 * (ball_.velocity() - 2 * (ball_.velocity().Dot(Vector3(0, 0, 1)) * Vector3(0, 0, 1))));
    }
    if (ball_.position().x() + ball_.radius() <= -40) {
        // left
        ball_.set_position(Point3(ball_.radius() - 40, ball_.position().y(), ball_.position().z()));
        ball_.set_velocity(0.8 * (ball_.velocity() - 2 * (ball_.velocity().Dot(Vector3(1, 0, 0)) * Vector3(1, 0, 0))));
    }
    if (ball_.position().x() + ball_.radius() >= 40) {
        // right
        ball_.set_position(Point3(40 - ball_.radius(), ball_.position().y(), ball_.position().z()));
        ball_.set_velocity(0.8 * (ball_.velocity() - 2 * (ball_.velocity().Dot(Vector3(-1, 0, 0)) * Vector3(-1, 0, 0))));
    }
    // ball gravity
    Vector3 gravity = 30 * Vector3(0, -1, 0);
    ball_.set_velocity(ball_.velocity() + gravity * dt);
    ball_.set_position(ball_.position() + dt * ball_.velocity());

    /* Car Routines */
    car_.set_speed(car_.speed() + 20.0f * dt);
    car_.set_forward(Vector3(dpad_dir[0], 0, -dpad_dir[1]));
    car_.set_position(car_.position() + car_.speed() * Vector3(dpad_dir[0], 0, -dpad_dir[1]) * dt);
    if (car_.position().z() + car_.collision_radius() >= 50) {
        // home
        car_.set_position(Point3(car_.position().x(), car_.position().y(), 50 - car_.collision_radius()));
    }
    if (car_.position().z() + car_.collision_radius() <= -50) {
        // away
        car_.set_position(Point3(car_.position().x(), car_.position().y(), car_.collision_radius() - 50));
    }
    if (car_.position().x() + car_.collision_radius() <= -40) {
        // left
        std::cout << "left collide" << std::endl;
        car_.set_position(Point3(car_.collision_radius() - 40, car_.position().y(), car_.position().z()));
    }
    if (car_.position().x() + car_.collision_radius() >= 40) {
        // right
        car_.set_position(Point3(40 - car_.collision_radius(), car_.position().y(), car_.position().z()));
    }

    /* Goal Routines */
    if (ball_.position().x() + ball_.radius() >= -20 && ball_.position().x() + ball_.radius() <= 20
        && ball_.position().y() - ball_.radius() >= 0 && ball_.position().y() - ball_.radius() <= 10
        && ball_.position().z() + ball_.radius() >= 50) {
        //Goal scenario: home
        ball_.Reset();
        car_.Reset();
    }
    if (ball_.position().x() + ball_.radius() >= -20 && ball_.position().x() + ball_.radius() <= 20
        && ball_.position().y() - ball_.radius() >= 0 && ball_.position().y() - ball_.radius() <= 10
        && ball_.position().z() + ball_.radius() <= -50) {
        //Goal scenario: away
        ball_.Reset();
        car_.Reset();
    }

}


void CarSoccer::InitOpenGL() {
    // Set up the camera in a good position to see the entire field
    projMatrix_ = Matrix4::Perspective(60, aspect_ratio(), 1, 1000);
    modelMatrix_ = Matrix4::LookAt(Point3(0,60,70), Point3(0,0,10), Vector3(0,1,0));
 
    // Set a background color for the screen (don't worry if you get a depricated warning on this line in OSX)
    glClearColor(0.8f, 0.8f, 0.8f, 1.0f);
    
    // Load some image files we'll use
    fieldTex_.InitFromFile(Platform::FindFile("pitch.png", searchPath_));
    crowdTex_.InitFromFile(Platform::FindFile("crowd.png", searchPath_));
}


void CarSoccer::DrawUsingOpenGL() {
    // Draw the crowd as a fullscreen background image
    quickShapes_.DrawFullscreenTexture(Color(1, 1, 1), crowdTex_);

    // Draw the car and the ball
    car_.Draw(quickShapes_, modelMatrix_, viewMatrix_, projMatrix_);
    ball_.Draw(quickShapes_, modelMatrix_, viewMatrix_, projMatrix_);

    // Draw the field with the field texture on it.
    Color col(16.0f / 255.0f, 46.0f / 255.0f, 9.0f / 255.0f);
    Matrix4 M = Matrix4::Translation(Vector3(0.0f, -0.201f, 0.0f)) * Matrix4::Scale(Vector3(50.0f, 1.0f, 60.0f));
    quickShapes_.DrawSquare(modelMatrix_ * M, viewMatrix_, projMatrix_, col);
    M = Matrix4::Translation(Vector3(0.0f, -0.2f, 0.0f)) * Matrix4::Scale(Vector3(40.0f, 1.0f, 50.0f));
    quickShapes_.DrawSquare(modelMatrix_ * M, viewMatrix_, projMatrix_, Color(1, 1, 1), fieldTex_);

    // You should add drawing the goals and the boundary of the playing area
    // using quickShapes_.DrawLines()

    // Bounding box
    std::vector<Point3> line;
    line.push_back(Point3(1.0, 0.0, 1.0));   line.push_back(Point3(1.0, 35.0, 1.0));
    line.push_back(Point3(-1.0, 0.0, -1.0)); line.push_back(Point3(-1.0, 35.0, -1.0));
    line.push_back(Point3(-1.0, 0.0, 1.0));  line.push_back(Point3(-1.0, 35.0, 1.0));
    line.push_back(Point3(1.0, 0.0, -1.0));  line.push_back(Point3(1.0, 35.0, -1.0));
    line.push_back(Point3(-1.0, 35.0, 1.0)); line.push_back(Point3(1.0, 35.0, 1.0));
    line.push_back(Point3(1.0, 35.0, 1.0));  line.push_back(Point3(1.0, 35.0, -1.0));
    line.push_back(Point3(1.0, 35.0, -1.0)); line.push_back(Point3(-1.0, 35.0, -1.0));
    line.push_back(Point3(-1.0, 35.0, -1.0)); line.push_back(Point3(-1.0, 35.0, 1.0));
    quickShapes_.DrawLines(modelMatrix_ * M, viewMatrix_, projMatrix_, Color(1, 1, 1, 0.1), line, QuickShapes::LinesType::LINES, 0.001);
    
    // Away Goal
    std::vector<Point3> awayBounds;
    awayBounds.push_back(Point3(20.0 / 80.0, 0.0, -1));
    awayBounds.push_back(Point3(20.0 / 80.0, 10.0, -1));
    awayBounds.push_back(Point3(-20.0 / 80.0, 10.0, -1));
    awayBounds.push_back(Point3(-20.0 / 80.0, 0.0, -1));
    quickShapes_.DrawLines(modelMatrix_ * M, viewMatrix_, projMatrix_, Color(0, 0, 1), awayBounds, QuickShapes::LinesType::LINE_LOOP, .01);

    std::vector<Point3> awayGrid;
    for (int i = 0; i < 10; i++) {
        //Vertial grid
        awayGrid.push_back(Point3(-20.0 / 80.0 + i / 20.0, 0.0, -1));
        awayGrid.push_back(Point3(-20.0 / 80.0 + i / 20.0, 10.0, -1));
    }
    for (int j = 0; j < 10; j++) {
        //Horz grid
        awayGrid.push_back(Point3(-20.0 / 80.0, j, -1));
        awayGrid.push_back(Point3(20.0 / 80.0, j, -1));
    } quickShapes_.DrawLines(modelMatrix_ * M, viewMatrix_, projMatrix_, Color(0, 0, 1, 0.2), awayGrid, QuickShapes::LinesType::LINES, .003);


    // Home Goal
    std::vector<Point3> homeBounds;
    homeBounds.push_back(Point3(20.0 / 80.0, 0.0, 1));
    homeBounds.push_back(Point3(20.0 / 80.0, 10.0, 1));
    homeBounds.push_back(Point3(-20.0 / 80.0, 10.0, 1));
    homeBounds.push_back(Point3(-20.0 / 80.0, 0.0, 1));
    quickShapes_.DrawLines(modelMatrix_ * M, viewMatrix_, projMatrix_, Color(1, 0, 0), homeBounds, QuickShapes::LinesType::LINE_LOOP, .01);
    
    std::vector<Point3> homeGrid;
    for (int i = 0; i < 10; i++) {
        //Vertial grid
        homeGrid.push_back(Point3(-20.0 / 80.0 + i / 20.0, 0.0, 1));
        homeGrid.push_back(Point3(-20.0 / 80.0 + i / 20.0, 10.0, 1));
    }
    for (int j = 0; j < 10; j++) {
        //Horz grid
        homeGrid.push_back(Point3(-20.0 / 80.0, j, 1));
        homeGrid.push_back(Point3(20.0 / 80.0, j, 1));
    } quickShapes_.DrawLines(modelMatrix_ * M, viewMatrix_, projMatrix_, Color(1, 0, 0, 0.2), homeGrid, QuickShapes::LinesType::LINES, .003);

}