/*
 * ComputerCore.cpp
 *
 *  Created on: Feb 22, 2018
 *      Author: vivienne
 */

#include "HangarsClientStd.h"

#include "GameAsset/GAFactory.h"

#include "AlphaEngine/GameAsset/Components/PhysicsComponent/RigidBodyComponent/RigidBodyComponent.h"

// Navigation Module
#include "GameAssets/Components/ComputerCore/ComputerCoreComponent.h"
#include "GameAssets/Components/PropulsionThruster/PropulsionThrusterComponent.h"
#include "GameAssets/Components/NavigationModule/NavigationModuleComponent.h"

#include "GameAsset/Components/CharacterComponent/CharacterComponent.h"

ComputerCoreComponent::ComputerCoreComponent(Context* context) :
        LogicComponent(context), m_ComputerCoreType(ComputerCore_NULL), m_Name(
                ""), m_pNavigationModule(nullptr), m_Initialized(false), m_ThrustLevel(
                0.5f), m_ThrustActive(false), m_pAutomatedVehicle(nullptr), m_MovementLongitude(
                0.0f), m_MovementLatitude(0.0f), m_MovementPitch(0.0f), m_MovementVertical(
                0.0f), m_AutomatedVehicleMode(AutomatedVehicleMode_NULL), m_Yaw(
                0.0f), m_qRotation(Quaternion::IDENTITY), m_pCharacter(nullptr) {
    m_CommandBuffer.clear();

}

ComputerCoreComponent::~ComputerCoreComponent() {

}

void ComputerCoreComponent::RegisterObject(Context* context) {
    context->RegisterFactory<ComputerCoreComponent>();
}

bool ComputerCoreComponent::LoadXML(const XMLElement& source,
        bool setInstanceDefault) {
    ResourceCache* cache = g_pApp->GetConstantResCache();

    // Get Name
    XMLElement node = source.GetChild("UniqueID");

    if (node) {
        m_Name = node.GetAttribute("value").CString();
    }

    // NEED TO CONVERT
    // Get Computer Core Type
    node = source.GetChild("ComputerCoreType");

    if (node) {
        m_ComputerCoreType = (ComputerCoreType) node.GetUInt("value");
    }

    // Set Loaded XML to true
    m_bLoadedXML = true;

    return true;
}

bool ComputerCoreComponent::SaveXML(XMLElement& dest) const {
    XMLElement componentNode = GAFactory::SaveComponentToXML(dest,
            GetTypeName(), node_);

    // Create name
    XMLElement childNode = componentNode.CreateChild("UniqueID");

    // Set the name
    childNode.SetString("value", String(m_Name.c_str()));

    // Set vehicle type
    childNode = componentNode.CreateChild("ComputerCoreType");

    // NEED TO CONVERT
    childNode.SetUInt("value", m_ComputerCoreType);

    return true;
}

// Process per frame
void ComputerCoreComponent::FixedUpdate(float timeStep) {
    // Do nothing--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
}

// Initialize
void ComputerCoreComponent::Initialize() {
    // Debug
    ALPHAENGINE_LOGINFO("Initialize Called");

    // Get the Navigation Module
    m_pNavigationModule = GetComponent<NavigationModuleComponent>();

    // Get Automated Vechicle Component
    m_pAutomatedVehicle = GetComponent<AutomatedVehicleComponent>();

    // Get attached propulsion thrusters
    PODVector<PropulsionThrusterComponent *> pPropulsionThrusters;

    // Get Thrusters
    GetNode()->GetComponents<PropulsionThrusterComponent>(pPropulsionThrusters,
            true);

    // Copy thrusters into memory
    if (pPropulsionThrusters.Size() == 0) {
        return;
    } else {
        if (m_pPropulsionThrusters.Size() == 0) {
            // loop thoop each thruster and update
            for (unsigned int i = 0; i < pPropulsionThrusters.Size(); i++) {
                // copy each thruster
                m_pPropulsionThrusters.Push(
                        (SharedPtr<PropulsionThrusterComponent> ) pPropulsionThrusters[i]);
            }

        }
    }

    // Subscribe to event
    SubscribeToEvent(E_POSTUPDATE,
            URHO3D_HANDLER(ComputerCoreComponent, HandlePostUpdate));

    // Create a cardinal table
    CreateCardinalTable();

}

// All Updates
void ComputerCoreComponent::Update(float timeStep) {
    // If command buffer
    if (m_CommandBuffer.size()) {
        ComputerCoreCommand currentCommand = m_CommandBuffer.front();

        unsigned int command = 0;

        // Output command
        if (currentCommand.commandCode != 0) {

            // conver tcommand code to
            command = currentCommand.commandCode;

            // erase data
            m_CommandBuffer.erase(m_CommandBuffer.begin());
        }

        // Ship Controls
        if (command == ShipControl_Pitch_Increase) {
            m_MovementPitch += .10;
            if (m_MovementPitch > 1.0f)
                m_MovementPitch = 1.0f;
        }

        if (command == ShipControl_Pitch_Decrease) {
            m_MovementPitch -= .10;
            if (m_MovementPitch < -1.0f)
                m_MovementPitch = -1.0f;
        }

        if (command == ShipControl_Longitude_Increase) {
            m_MovementLongitude += .10;
            if (m_MovementLongitude > 1.0f)
                m_MovementLongitude = 1.0f;
        }

        if (command == ShipControl_Longitude_Decrease) {
            m_MovementLongitude -= .10;
            if (m_MovementLongitude < -1.0f)
                m_MovementLongitude = -1.0f;
        }

        if (command == ShipControl_Vertical_Increase) {
            m_MovementVertical += .10;
            if (m_MovementVertical > 1.0f)
                m_MovementVertical = 1.0f;
        }

        if (command == ShipControl_Vertical_Decrease) {
            m_MovementVertical -= .10;
            if (m_MovementVertical < -1.0f)
                m_MovementVertical = -1.0f;
        }

        if (command == ShipControl_Latitude_Increase) {
            m_MovementLatitude += .10;
            if (m_MovementLatitude > 1.0f)
                m_MovementLatitude = 1.0f;
        }

        if (command == ShipControl_Latitude_Decrease) {
            m_MovementLatitude -= .10;
            if (m_MovementLatitude < -1.0f)
                m_MovementLatitude = -1.0f;
        }

        if (command == ShipControl_Longitude_Increase) {
            m_MovementLongitude += .10;
            if (m_MovementLongitude > 1.0f)
                m_MovementLongitude = 1.0f;
        }

        if (command == ShipControl_Longitude_Decrease) {
            m_MovementLongitude -= .10;
            if (m_MovementLongitude < -1.0f)
                m_MovementLongitude = -1.0f;
        }

        if (command == ShipControl_Vertical_Increase) {
            m_MovementVertical += .10;
            if (m_MovementVertical > 1.0f)
                m_MovementVertical = 1.0f;
        }

        if (command == ShipControl_Vertical_Decrease) {
            m_MovementVertical -= .10;
            if (m_MovementVertical < -1.0f)
                m_MovementVertical = -1.0f;
        }

        if (command == ShipControl_Yaw_Decrease) {
            m_Yaw -= .10;
            if (m_Yaw < -1.0f)
                m_Yaw = -1.0f;
        }

        if (command == ShipControl_Yaw_Increase) {
            m_Yaw += .10;
            if (m_Yaw > 1.0f)
                m_Yaw = 1.0f;
        }

        if (command == ShipControl_AutomatedMode) {
            if (m_AutomatedVehicleMode == AutomatedVehicleMode_NULL) {
                //ALPHAENGINE_LOGINFO("AutomatedVehicleMode Enabled");
                m_AutomatedVehicleMode = AutomatedVehicleMode_Enable;
            } else {
                //ALPHAENGINE_LOGINFO("AutomatedVehicleMode Null");
                m_AutomatedVehicleMode = AutomatedVehicleMode_NULL;
            }
        }

        if (command == ShipControl_FullStop) {
            // Reset all forces
            m_MovementLatitude = -1.0f;
            m_MovementLongitude = -1.0f;
            m_MovementVertical = -1.0f;
        }

        // Clear Command Buffer
        //m_CommandBuffer.clear();
    }

    // Convert Movement to Velocity
    ConvertMovementToVelocity();
}

// Set State of modules
void ComputerCoreComponent::SetState(bool state) {
    // If navigation module
    if (m_pNavigationModule) {
        m_pNavigationModule->SetState(state);
    }
}

// Test Key Press message
void ComputerCoreComponent::KeyPress(String message) {
    // output message
}

// Issue Command
void ComputerCoreComponent::IssueCommand(ComputerCoreCommand command) {
    // Add command
    m_CommandBuffer.push_back(command);
}

void ComputerCoreComponent::HandlePostUpdate(StringHash eventType,
        VariantMap& eventData) {
    if (m_pAutomatedVehicle) {
        float timeStep = eventData[PostUpdate::P_TIMESTEP].GetFloat();

        if (m_LinearVelocity != Vector3::ZERO) {
            // Apply Linear Velocity

            if (m_LinearVelocity.x_ == -1.0f && m_LinearVelocity.y_ == -1.0f
                    && m_LinearVelocity.z_ == -1.0f) {

                Vector3 currentLinear =
                        m_pAutomatedVehicle->GetLinearVelocity();

                if (currentLinear != Vector3::ZERO) {
                    m_pAutomatedVehicle->GetNode()->GetComponent<
                            RigidBodyComponent>()->SetLinearVelocity(
                            Vector3::ZERO);
                    m_pAutomatedVehicle->GetNode()->GetComponent<
                            RigidBodyComponent>()->SetAngularVelocity(
                            Vector3::ZERO);

                }
            } else {
                // Apply impulse to vehicle linear velocity
                m_pAutomatedVehicle->ApplyImpulse(m_LinearVelocity * 4);
            }

            // Reset all forces
            m_MovementLatitude = m_MovementLongitude = m_MovementVertical =
                    0.0f;
        }

        // Enable Vehicle Mode Balance
        if (m_AutomatedVehicleMode == AutomatedVehicleMode_Enable
                || m_LinearVelocity != Vector3::ZERO || m_Yaw != 0.0f) {

            // If autoenabled buggie
            if (m_AutomatedVehicleMode == AutomatedVehicleMode_Enable) {
                // Get Rotation in Quaternion
                Quaternion rotation = m_pAutomatedVehicle->GetRotation();

                // Create a Identity which is one
                Quaternion test = Quaternion::IDENTITY;

                // Create a angle axis using the current vehicle rotation specifically the rigidbody
                test.FromAngleAxis(rotation.YawAngle(), Vector3::UP);

                // Times the rotations by the inverse
                Quaternion delta = test
                        * m_pAutomatedVehicle->GetRotation().Inverse();

                // Create blank angle and axis
                float angle = 0.0f;
                Vector3 axis = Vector3::ZERO;

                // Convert delta time to axis and angle
                ToAngleAxis(delta, angle, axis);

                // If axis x and axiz z equal to aero skip adjustment
                if (axis.x_ == 0.0f) {
                    // do nothing
                } else {
                    // Multiply a normalized axis using timestep and amount
                    Vector3 test = axis.Normalized() * 0.95f * timeStep;

                    // Apply angular velocity to vehicle
                    m_pAutomatedVehicle->ApplyAngularVelocity(test);
                }
            }

            //  If player control yaw is not zero
            if (m_Yaw != 0.0f) {
                // Apply Rotation
                // Generate Quaternion from Yaw multiply by 10
                m_pAutomatedVehicle->ApplyRotate(
                        Quaternion(m_Yaw * 10, Vector3::UP));

                // Reset player control yaw to zero
                m_Yaw = 0.0f;
            }
        }
    } else {
        //ALPHAENGINE_LOGINFO("Post Update Failed. No Automated Vehicle");
    }
}

void ComputerCoreComponent::CreateCardinalTable() {
// Sum of available thrust
    float SumAvailableThrust = 0;

// Cardinal Table for propulsion
    for (unsigned int i = CardinalDirectionVert_UP;
            i < CardinalDirectionVert_MAX; i++) {
        SumAvailableThrust = 0;

        for (unsigned int j = CardinalDirectionHorz_NORTH;
                j < CardinalDirectionHorz_MAX; j++) {

            // Clear all current data
            m_CardinalTable[i][j].Thrusters.Clear();
            m_CardinalTable[i][j].AvailableThrust = 0;

            // Loop through each thruster
            for (unsigned int thruster = 0;
                    thruster < m_pPropulsionThrusters.Size(); thruster++) {
                SumAvailableThrust +=
                        m_pPropulsionThrusters[j]->GetForceAvailable(
                                (CardinalDirectionVetical) i,
                                (CardinalDirectionHorizontal) j);
            }

        }
    }
}

// Convert Movement to Velocity
void ComputerCoreComponent::ConvertMovementToVelocity() {
// Create a zero vector
    Vector3 LinearVelocity = Vector3::ZERO;

// Calculate impulse amount by Latitiude, Vertical, and Longititude
    LinearVelocity = Vector3(m_MovementLatitude * 2, m_MovementVertical * 2,
            m_MovementLongitude * 2);

// Set Linear Velocity Impulse
    m_LinearVelocity = LinearVelocity;

    if (m_MovementLatitude == -1.0f && m_MovementVertical == -1.0f
            && m_MovementLongitude == -1.0f) {
        m_LinearVelocity = Vector3(-1.0f, -1.0f, -1.0f);
    }
}

void ComputerCoreComponent::ToAngleAxis(Quaternion q1, float & angle,
        Vector3 & axis) {
    float retAngle = 2 * std::acos(q1.w_);
    double s = std::sqrt(1 - q1.w_ * q1.w_);

    Vector3 retAxis;

    if (s < 0.001) {
        retAxis.x_ = q1.x_;
        retAxis.y_ = q1.y_;
        retAxis.z_ = q1.z_;
    } else {
        retAxis.x_ = q1.x_ / s;
        retAxis.y_ = q1.y_ / s;
        retAxis.z_ = q1.z_ / s;
    }

    angle = retAngle;
    axis = retAxis;
}

void ComputerCoreComponent::SetCharacter(CharacterComponent * character) {
    m_pCharacter = character;
}

// Convert input to thrust per amount
/*for (unsigned int i = CardinalDirectionVert_UP;
 i < CardinalDirectionVert_MAX; i++) {

 if (i == CardinalDirectionVert_UP || CardinalDirectionVert_DOWN) {
 continue;
 }

 for (unsigned int j = CardinalDirectionHorz_NORTH;
 j < CardinalDirectionHorz_MAX; j++) {

 // Calculate per direciton
 if (j == CardinalDirectionHorz_NORTH
 && m_MovementLongitude > 0.0f) {
 LinearVelocity += Vector3::FORWARD
 * m_CardinalTable[i][j].AvailableThrust
 * m_MovementLongitude;
 }

 // Calculate per direciton
 if (j == CardinalDirectionHorz_SOUTH
 && m_MovementLongitude < 0.0f) {
 LinearVelocity += Vector3::BACK
 * m_CardinalTable[i][j].AvailableThrust
 * -(m_MovementLongitude);

 }

 // Calculate per direciton
 if (j == CardinalDirectionHorz_EAST && m_MovementLatitude > 0.0f) {
 LinearVelocity += Vector3::RIGHT
 * m_CardinalTable[i][j].AvailableThrust
 * m_MovementLongitude;

 }

 // Calculate per direciton
 if (j == CardinalDirectionHorz_WEST && m_MovementLatitude < 0.0f) {
 LinearVelocity += Vector3::LEFT
 * m_CardinalTable[i][j].AvailableThrust
 * -(m_MovementLongitude);
 }
 }
 }*/