#include <rofi/connector.hpp>

#include <rofi/voxel.hpp>

namespace rofi
{

Connector::Connector(const Connector &other)
{
    node = Node::copy_deep(other.node);
    voxel_side = other.voxel_side;
}

Connector::Connector(const Connector &&other)
{
    node = std::move(other.node);
    voxel_side = std::move(other.voxel_side);
}

Connector& Connector::operator=(const Connector &other)
{
    node = Node::copy_deep(other.node);
    voxel_side = other.voxel_side;

    return *this;
}

Connector& Connector::operator=(const Connector &&other)
{
    node = std::move(other.node);
    voxel_side = std::move(other.voxel_side);

    return *this;
}

connector_ptr Connector::copy(connector_ptr other_connector)
{
    if (auto fixed_con = std::dynamic_pointer_cast<FixedConnector>(other_connector))
        return FixedConnector::copy(fixed_con);
    if (auto open_con = std::dynamic_pointer_cast<OpenConnector>(other_connector))
        return OpenConnector::copy(open_con);
    if (auto rot_con = std::dynamic_pointer_cast<RotationConnector>(other_connector))
        return RotationConnector::copy(rot_con);
    if (auto shared_rot_con = std::dynamic_pointer_cast<SharedRotationConnector>(other_connector))
        return SharedRotationConnector::copy(shared_rot_con);

    ASSUMPTION(false);
    return std::make_shared<FixedConnector>(Node::create());
}

bool Connector::shares_module_link(connector_ptr fst_connector, connector_ptr snd_connector)
{
    auto fst_connection = fst_connector->getConnection();
    auto snd_connection = snd_connector->getConnection();

    if (!(!fst_connection.first.owner_before(snd_connection.first) && !snd_connection.first.owner_before(fst_connection.first)
        || !fst_connection.first.owner_before(snd_connection.second) && !snd_connection.second.owner_before(fst_connection.first)
        || !fst_connection.second.owner_before(snd_connection.first) && !snd_connection.first.owner_before(fst_connection.second)
        || !fst_connection.second.owner_before(snd_connection.second) && !snd_connection.second.owner_before(fst_connection.second)))
        return false;

    auto [ff, fs] = fst_connection;
    auto [sf, ss] = snd_connection;
    auto fst_fst = !ff.expired() ? std::dynamic_pointer_cast<ModuleLink>(ff.lock()) : nullptr;
    auto fst_snd = !fs.expired() ? std::dynamic_pointer_cast<ModuleLink>(fs.lock()) : nullptr;
    auto snd_fst = !sf.expired() ? std::dynamic_pointer_cast<ModuleLink>(sf.lock()) : nullptr;
    auto snd_snd = !ss.expired() ? std::dynamic_pointer_cast<ModuleLink>(ss.lock()) : nullptr;
    
    if (((fst_fst && snd_fst) && (fst_fst == snd_fst))
        || ((fst_fst && snd_snd) && (fst_fst == snd_snd))
        || ((fst_snd && snd_fst) && (fst_snd == snd_fst))
        || ((fst_snd && snd_snd) && (fst_snd == snd_snd)))
        return true;

    return false;
}

cardinal Connector::get_mutual_orientation(connector_ptr source_connector, connector_ptr destination_connector)
{
    // ASSUMPTION(Connector::shares_module_link(source_connector, destination_connector));

    // auto source_north = source_connector->getNode()->getRotationAxisXWorld();
    // auto destination_north = destination_connector->getNode()->getRotationAxisXWorld();
    auto source_rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(source_connector);
    bool source_is_head = source_rot_con && source_rot_con->isHead();
    auto destination_rot_con = std::dynamic_pointer_cast<rofi::RotationConnector>(destination_connector);
    bool destination_is_head = destination_rot_con && destination_rot_con->isHead();
    const auto source_north = source_is_head 
                               ? -source_connector->getNode()->getRotationAxisXWorld()
                               : source_connector->getNode()->getRotationAxisXWorld();
    const auto destination_north = destination_is_head 
                                    ? -destination_connector->getNode()->getRotationAxisXWorld()
                                    : destination_connector->getNode()->getRotationAxisXWorld();

    auto oriented_angle = glm::degrees(glm::orientedAngle(destination_north, 
                                                          source_north, 
                                                          destination_connector->getNode()->getRotationAxisZWorld()));                                                 

    return degree_to_cardinal(oriented_angle);
}

bool Connector::hasModuleLink() const
{ 
    return (!connection.first.expired() && std::dynamic_pointer_cast<ModuleLink>(connection.first.lock()))
           || (!connection.second.expired() && std::dynamic_pointer_cast<ModuleLink>(connection.second.lock()));
}

bool Connector::isOnPad() const
{
    return (!connection.first.expired() && std::dynamic_pointer_cast<PadBoard>(connection.first.lock()))
           || (!connection.second.expired() && std::dynamic_pointer_cast<PadBoard>(connection.second.lock()));
}

voxel_weak_ptr Connector::getModuleLink() const
{
    ASSUMPTION(hasModuleLink());

    if (!connection.first.expired() && std::dynamic_pointer_cast<ModuleLink>(connection.first.lock()))
        return connection.first;
    if (!connection.second.expired() && std::dynamic_pointer_cast<ModuleLink>(connection.second.lock()))
        return connection.second;

    ASSUMPTION(false);
    return connection.first;
}

void Connector::connectVoxel(side s, voxel_weak_ptr voxel)
{
    ASSUMPTION(!isFull());

    (connection.first.expired() ? connection.first : connection.second) = voxel;

    ASSUMPTION(!connection.first.expired() || !connection.second.expired());

    (voxel_side.first == side::undefined ? voxel_side.first : voxel_side.second) = s;

    ASSUMPTION(connection.first.lock() != connection.second.lock());

    if (voxel_side.first != side::undefined && voxel_side.second != side::undefined)
    {
        ASSUMPTION(!connection.first.expired() && !connection.second.expired());
    }  
}
void Connector::disconnectVoxel(voxel_weak_ptr voxel)
{
    ASSUMPTION(connection.first.lock() == voxel.lock() || connection.second.lock() == voxel.lock());
    
    if(connection.first.lock() == voxel.lock())
    {
        connection.first.reset();
        voxel_side.first = side::undefined;
    }
    else
    {
        connection.second.reset();
        voxel_side.second = side::undefined;
    }
}

void Connector::disconnectVoxel(side s)
{
    ASSUMPTION(voxel_side.first == s || voxel_side.second == s);

    if (voxel_side.first == s)
        disconnectVoxel(connection.first);
    else
        disconnectVoxel(connection.second);
}
voxel_weak_ptr Connector::getOppositeConnection()
{
    ASSUMPTION(!(connection.first.expired() && connection.second.expired()));
    ASSUMPTION(!(!connection.first.expired() && !connection.second.expired()));

    return !connection.first.expired() ? connection.first : connection.second;
}

/* Argument s: The side from which we are accessing the connector;
   Returns voxel with opposite 's'. */
voxel_weak_ptr Connector::getOppositeConnection(side s)
{
    ASSUMPTION(isFull());
    ASSUMPTION(voxel_side.first == s || voxel_side.second == s);

    return voxel_side.first != s ? connection.first : connection.second;
}

/* FIXED CONNECTOR */
FixedConnector::FixedConnector(const FixedConnector &other)
: Connector(other) {}

FixedConnector::FixedConnector(const FixedConnector &&other)
: Connector(other) {}

FixedConnector& FixedConnector::operator=(const FixedConnector &other)
{
    Connector::operator=(other);
    return *this;
}

FixedConnector& FixedConnector::operator=(const FixedConnector &&other)
{
    Connector::operator=(std::move(other));
    return *this;
}

std::shared_ptr<FixedConnector> FixedConnector::copy(std::shared_ptr<FixedConnector> other_fixed_connector)
{
    return std::shared_ptr<FixedConnector>{ new FixedConnector(*other_fixed_connector) };
}

/* OPEN CONNECTOR */
OpenConnector::OpenConnector(const OpenConnector &other)
: Connector(other) {}

OpenConnector::OpenConnector(const OpenConnector &&other)
: Connector(other) {}

OpenConnector& OpenConnector::operator=(const OpenConnector &other)
{
    Connector::operator=(other);
    return *this;
}

OpenConnector& OpenConnector::operator=(const OpenConnector &&other)
{
    Connector::operator=(other);
    return *this;
}

std::shared_ptr<OpenConnector> OpenConnector::copy(std::shared_ptr<OpenConnector> other_open_connector)
{
    return std::shared_ptr<OpenConnector>{ new OpenConnector(*other_open_connector) };
}

/* ROTATION CONNECTOR */

RotationConnector::RotationConnector(const RotationConnector &other) 
: Connector(other)
{
    rotation_axis = other.rotation_axis;
    is_head = other.is_head;

    semantic_type = other.semantic_type;
    neg_rotation_limit = other.neg_rotation_limit;
    pos_rotation_limit = other.pos_rotation_limit;
}

RotationConnector::RotationConnector(const RotationConnector &&other)
: Connector(other)
{    
    rotation_axis = std::move(other.rotation_axis);
    is_head = std::move(other.is_head);

    semantic_type = std::move(other.semantic_type);
    neg_rotation_limit = std::move(other.neg_rotation_limit);
    pos_rotation_limit = std::move(other.pos_rotation_limit);
}

RotationConnector& RotationConnector::operator=(const RotationConnector &other)
{
    Connector::operator=(other);

    rotation_axis = other.rotation_axis;
    is_head = other.is_head;

    semantic_type = other.semantic_type;
    neg_rotation_limit = other.neg_rotation_limit;
    pos_rotation_limit = other.pos_rotation_limit;

    return *this;
}

RotationConnector& RotationConnector::operator=(const RotationConnector &&other)
{
    Connector::operator=(other);

    rotation_axis = std::move(other.rotation_axis);
    is_head = std::move(other.is_head);

    semantic_type = std::move(other.semantic_type);
    neg_rotation_limit = std::move(other.neg_rotation_limit);
    pos_rotation_limit = std::move(other.pos_rotation_limit);

    return *this;
}

std::shared_ptr<RotationConnector> RotationConnector::copy(std::shared_ptr<RotationConnector> other_rotation_connector)
{
    return std::shared_ptr<RotationConnector>{ new RotationConnector(*other_rotation_connector) };
}

bool RotationConnector::isParentVoxel(voxel_weak_ptr voxel) const
{
    auto locked_voxel = voxel.lock();
    ASSUMPTION(locked_voxel);

    return locked_voxel == connection.first.lock();
}

/* SHARED ROTATION CONNECTOR */

SharedRotationConnector::SharedRotationConnector(const SharedRotationConnector &other)
: Connector(other)
{
    rotation_axis = other.rotation_axis;
    modulo = other.modulo;
}

SharedRotationConnector::SharedRotationConnector(const SharedRotationConnector &&other)
: Connector(other)
{
    rotation_axis = std::move(other.rotation_axis);
    modulo = std::move(other.modulo);
}

SharedRotationConnector& SharedRotationConnector::operator=(const SharedRotationConnector &other)
{
    Connector::operator=(other);

    rotation_axis = other.rotation_axis;
    modulo = other.modulo;

    return *this;
}

SharedRotationConnector& SharedRotationConnector::operator=(const SharedRotationConnector &&other)
{
    Connector::operator=(other);

    rotation_axis = std::move(other.rotation_axis);
    modulo = std::move(other.modulo);

    return *this;
}

std::shared_ptr<SharedRotationConnector> SharedRotationConnector::copy(std::shared_ptr<SharedRotationConnector> other_shared_rotation_connector)
{
    return std::shared_ptr<SharedRotationConnector>{ new SharedRotationConnector(*other_shared_rotation_connector) };
}

side vec_to_enum(glm::vec3 local_direction)
{
    using enum side;
    float epsilon = 0.0001f;

    if (glm::all(glm::epsilonEqual(local_direction, glm::vec3(1.0f, 0.0f, 0.0f), epsilon)))
        return x_pos;
    if (glm::all(glm::epsilonEqual(local_direction, glm::vec3(-1.0f, 0.0f, 0.0f), epsilon)))
        return x_neg;
    if (glm::all(glm::epsilonEqual(local_direction, glm::vec3(0.0f, 1.0f, 0.0f), epsilon)))
        return y_pos;
    if (glm::all(glm::epsilonEqual(local_direction, glm::vec3(0.0f, -1.0f, 0.0f), epsilon)))
        return y_neg;
    if (glm::all(glm::epsilonEqual(local_direction, glm::vec3(0.0f, 0.0f, 1.0f), epsilon)))
        return z_pos;
    if (glm::all(glm::epsilonEqual(local_direction, glm::vec3(0.0f, 0.0f, -1.0f), epsilon)))
        return z_neg;

    ASSUMPTION(false);
    // MSVC throws warning otherwise
    return undefined;
}

glm::vec3 enum_to_vec(side s)
{
    using enum side;
    switch(s)
    {
        case x_pos:
            return glm::vec3(1.0f, 0.0f, 0.0f);
        case x_neg:
            return glm::vec3(-1.0f, 0.0f, 0.0f);
        case y_pos:
            return glm::vec3(0.0f, 1.0f, 0.0f);
        case y_neg:
            return glm::vec3(0.0f, -1.0f, 0.0f);
        case z_pos:
            return glm::vec3(0.0f, 0.0f, 1.0f);
        case z_neg:
            return glm::vec3(0.0f, 0.0f, -1.0f);
        default:
            ASSUMPTION(false);
            // MSVC throws warning otherwise
            return glm::vec3();
    }
}

cardinal degree_to_cardinal(float degree_angle)
{
    // NOTE: This epsilon value is this large, because the editor refused to connect
    //       connectors at an angle of 89.9989548 degrees, which I believe was a product
    //       of the floating-point arithmetic
    // NOTE 2: It also computed an angle of 0.0279764552 from a valid imported configuration

    float epsilon = 0.05f;
    if (glm::epsilonEqual(degree_angle, 0.0f, epsilon))
        return North;
    if (glm::epsilonEqual(degree_angle, 90.0f, epsilon))
        return East;
    if (glm::epsilonEqual(degree_angle, 180.0f, epsilon) || glm::epsilonEqual(degree_angle, -180.0f, epsilon))
        return South;
    if (glm::epsilonEqual(degree_angle, -90.0f, epsilon))
        return West;
    
    return Invalid;
}
float cardinal_to_degree(cardinal orientation)
{
    switch(orientation)
    {
        using enum cardinal;
        case North:
            return 0.0f;
        case East:
            return 90.0f;
        case South:
            return 180.0f;
        case West:
            return -90.0f;
        default:
            ASSUMPTION(false);
            return 0.0f;
    }
}

}