#include "controller.hpp"
#include "config.hpp"
#include "logger.hpp"
#include <unitree/robot/channel/channel_factory.hpp>
#include <unitree/common/thread/thread.hpp>

namespace custom {

Controller::Controller(const std::string& networkInterface)
    : networkInterface_(networkInterface), running_(false), initialized_(false) {
}

Controller::~Controller() {
    if (running_) {
        stop();
    }
    
    LOG_INFO("Shutting down robot controller");
    sportClient_.reset();
    obstacleClient_.reset();
    initialized_ = false;
}

bool Controller::initialize() {
    try {
        LOG_INFO("Initializing robot controller with interface: " + networkInterface_);
        
        unitree::robot::ChannelFactory::Instance()->Init(0, networkInterface_);
        
        sportClient_ = std::make_unique<unitree::robot::go2::SportClient>();
        sportClient_->SetTimeout(10.0f);
        sportClient_->Init();
        
        obstacleClient_ = std::make_unique<unitree::robot::go2::ObstaclesAvoidClient>();
        obstacleClient_->SetTimeout(3.0f);
        obstacleClient_->Init();
        
        
        initialized_ = true;
        LOG_INFO("Robot controller initialized successfully");
        return true;
        
    } catch (const std::exception& e) {
        LOG_ERROR("Failed to initialize robot controller: " + std::string(e.what()));
        return false;
    }
}

bool Controller::start() {
    if (!initialized_) {
        LOG_ERROR("Cannot start: robot controller not initialized");
        return false;
    }
    
    if (running_) {
        LOG_WARN("Robot controller already running");
        return true;
    }
    
    LOG_INFO("Starting robot controller");
    
    running_ = true;
    
    LOG_INFO("Robot controller started successfully");
    return true;
}

bool Controller::stop() {
    if (!running_) {
        return true;
    }
    
    LOG_INFO("Stopping robot controller");
    
    running_ = false;
    
    LOG_INFO("Robot controller stopped");
    return true;
}




bool Controller::StandUp() {
    LOG_INFO("Standing up");
    try {
        return sportClient_ && sportClient_->StandUp() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Stand up failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::StandDown() {
    LOG_INFO("Standing down");
    try {
        return sportClient_ && sportClient_->StandDown() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Stand down failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::Sit() {
    LOG_INFO("Sitting");
    try {
        return sportClient_ && sportClient_->Sit() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Sit failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::Lie() {
    LOG_INFO("Lying down");
    try {
        return sportClient_ && sportClient_->StandDown() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Lie down failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::Damp() {
    LOG_INFO("Damping");
    try {
        return sportClient_ && sportClient_->Damp() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Damp failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::RecoveryStand() {
    LOG_INFO("Executing recovery stand");
    
    try {
        if (sportClient_) {
            int32_t result = sportClient_->RecoveryStand();
            return result == 0;
        }
        return false;
    } catch (const std::exception& e) {
        LOG_ERROR("Recovery stand failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::StopMove() {
    try {
        return sportClient_ && sportClient_->StopMove() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Stop move failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::BalanceStand() {
    try {
        return sportClient_ && sportClient_->BalanceStand() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Balance stand failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::Dance1() {
    try {
        return sportClient_ && sportClient_->Dance1() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Dance1 failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::Dance2() {
    try {
        return sportClient_ && sportClient_->Dance2() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Dance2 failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::Hello() {
    try {
        return sportClient_ && sportClient_->Hello() == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Hello failed: " + std::string(e.what()));
        return false;
    }
}




bool Controller::SwitchSet(bool enable) {
    try {
        return obstacleClient_ && obstacleClient_->SwitchSet(enable) == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Switch set failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::SwitchGet(bool& enable) {
    try {
        return obstacleClient_ && obstacleClient_->SwitchGet(enable) == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Switch get failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::UseRemoteCommandFromApi(bool isRemoteCommandsFromApi) {
    try {
        return obstacleClient_ && obstacleClient_->UseRemoteCommandFromApi(isRemoteCommandsFromApi) == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Use remote command from api failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::MoveToAbsolutePosition(float x, float y, float yaw) {
    try {
        return obstacleClient_ && obstacleClient_->MoveToAbsolutePosition(x, y, yaw) == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Move to absolute position failed: " + std::string(e.what()));
        return false;
    }
}

bool Controller::MoveToIncrementPosition(float x, float y, float yaw) {
    try {
        return obstacleClient_ && obstacleClient_->MoveToIncrementPosition(x, y, yaw) == 0;
    } catch (const std::exception& e) {
        LOG_ERROR("Move to increment position failed: " + std::string(e.what()));
        return false;
    }
}

} // namespace custom