#include "WaypointModule.h"
#include "NodeDB.h"
#include "PowerFSM.h"
#include "configuration.h"
#include "graphics/draw/CompassRenderer.h"

#if HAS_SCREEN
#include "gps/RTC.h"
#include "graphics/Screen.h"
#include "graphics/TimeFormatters.h"
#include "graphics/draw/NodeListRenderer.h"
#include "main.h"
#endif

WaypointModule *waypointModule;

static inline float degToRad(float deg) { return deg * PI / 180.0f; }
static inline float radToDeg(float rad) { return rad * 180.0f / PI; }

ProcessMessage WaypointModule::handleReceived(const meshtastic_MeshPacket &mp)
{
#if defined(DEBUG_PORT) && !defined(DEBUG_MUTE)
    auto &p = mp.decoded;
    LOG_INFO("Received waypoint msg from=0x%0x, id=0x%x, msg=%.*s", mp.from, mp.id, p.payload.size, p.payload.bytes);
#endif
    // We only store/display messages destined for us.
    // Keep a copy of the most recent text message.
    devicestate.rx_waypoint = mp;
    devicestate.has_rx_waypoint = true;

    powerFSM.trigger(EVENT_RECEIVED_MSG);

#if HAS_SCREEN

    UIFrameEvent e;

    // New or updated waypoint: focus on this frame next time Screen::setFrames runs
    if (shouldDraw()) {
        requestFocus();
        e.action = UIFrameEvent::Action::REGENERATE_FRAMESET;
    }

    // Deleting an old waypoint: remove the frame quietly, don't change frame position if possible
    else
        e.action = UIFrameEvent::Action::REGENERATE_FRAMESET_BACKGROUND;

    notifyObservers(&e);

#endif

    return ProcessMessage::CONTINUE; // Let others look at this message also if they want
}

#if HAS_SCREEN
bool WaypointModule::shouldDraw()
{
#if !MESHTASTIC_EXCLUDE_WAYPOINT
    if (!screen || !devicestate.has_rx_waypoint) return false;

    meshtastic_Waypoint wp{};  // <- replaces memset
    if (pb_decode_from_bytes(devicestate.rx_waypoint.decoded.payload.bytes,
                             devicestate.rx_waypoint.decoded.payload.size,
                             &meshtastic_Waypoint_msg, &wp)) {
        return wp.expire > getTime();
    }
    return false;  // no LOG_ERROR, no flag writes
#else
    return false;
#endif
}

/// Draw the last waypoint we received
void WaypointModule::drawFrame(OLEDDisplay *display, OLEDDisplayUiState *state, int16_t x, int16_t y)
{
    if (!screen) return;
    // Prepare to draw
    display->setFont(FONT_SMALL);
    display->setTextAlignment(TEXT_ALIGN_LEFT);
    const int w = display->getWidth();
    const int h = display->getHeight();

    // Handle inverted display
    if (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_INVERTED)
        display->fillRect(x, y, w, FONT_HEIGHT_SMALL);

    // Decode the waypoint
    const meshtastic_MeshPacket &mp = devicestate.rx_waypoint;
    meshtastic_Waypoint wp{};
    if (!pb_decode_from_bytes(mp.decoded.payload.bytes, mp.decoded.payload.size, &meshtastic_Waypoint_msg, &wp)) {
        devicestate.has_rx_waypoint = false;
        return;
    }

    // Get timestamp info. Will pass as a field to drawColumns
    char lastStr[20];
    getTimeAgoStr(sinceReceived(&mp), lastStr, sizeof(lastStr));

    // Will contain distance information, passed as a field to drawColumns
    char distStr[20];

    // Get our node, to use our own position
    meshtastic_NodeInfoLite *ourNode = nodeDB->getMeshNode(nodeDB->getNodeNum());

    // Text fields to draw (left of compass)
    // Last element must be NULL. This signals the end of the char*[] to drawColumns
    const char *fields[] = {"Waypoint", lastStr, wp.name, wp.description, distStr, nullptr};

    // Dimensions / co-ordinates for the compass/circle
    const uint16_t compassDiam = graphics::CompassRenderer::getCompassDiam(w, h);
    const int16_t compassX = x + w - (compassDiam / 2) - 5;
    const int16_t compassY = (config.display.displaymode == meshtastic_Config_DisplayConfig_DisplayMode_DEFAULT)
                            ? y + h / 2
                            : y + FONT_HEIGHT_SMALL + (h - FONT_HEIGHT_SMALL) / 2;


    // If our node has a position:
    if (ourNode && (nodeDB->hasValidPosition(ourNode) || screen->hasHeading())) {
        const meshtastic_PositionLite &op = ourNode->position;
        float myHeading;
        if (uiconfig.compass_mode == meshtastic_CompassMode_FREEZE_HEADING) {
            myHeading = 0;
        } else {
            if (screen->hasHeading())
                myHeading = degToRad(screen->getHeading());
            else
                myHeading = screen->estimatedHeading(DegD(op.latitude_i), DegD(op.longitude_i));
        }
        graphics::CompassRenderer::drawCompassNorth(display, compassX, compassY, myHeading, (compassDiam / 2));

        // Compass bearing to waypoint
        float bearingToOther =
            GeoCoord::bearing(DegD(op.latitude_i), DegD(op.longitude_i), DegD(wp.latitude_i), DegD(wp.longitude_i));
        // If the top of the compass is a static north then bearingToOther can be drawn on the compass directly
        // If the top of the compass is not a static north we need adjust bearingToOther based on heading
        if (uiconfig.compass_mode != meshtastic_CompassMode_FREEZE_HEADING)
            bearingToOther -= myHeading;
        graphics::CompassRenderer::drawNodeHeading(display, compassX, compassY, compassDiam, bearingToOther);

        float bearingToOtherDegrees = (bearingToOther < 0) ? bearingToOther + 2 * PI : bearingToOther;
        bearingToOtherDegrees = radToDeg(bearingToOtherDegrees);

        // Distance to Waypoint
        float d = GeoCoord::latLongToMeter(DegD(wp.latitude_i), DegD(wp.longitude_i), DegD(op.latitude_i), DegD(op.longitude_i));
        if (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) {
            float feet = d * METERS_TO_FEET;
            snprintf(distStr, sizeof(distStr),
                    feet < (2 * MILES_TO_FEET) ? "%.0fft   %.0f°" : "%.1fmi   %.0f°",
                    feet < (2 * MILES_TO_FEET) ? feet : feet / MILES_TO_FEET, bearingToOtherDegrees);
        } else {
            snprintf(distStr, sizeof(distStr),
                    d < 2000 ? "%.0fm   %.0f°" : "%.1fkm   %.0f°",
                    d < 2000 ? d : d / 1000, bearingToOtherDegrees);
        }
    }

    else {
        display->drawString(compassX - FONT_HEIGHT_SMALL / 4, compassY - FONT_HEIGHT_SMALL / 2, "?");

        // ? in the distance field
        snprintf(distStr, sizeof(distStr), "? %s ?°",
         (config.display.units == meshtastic_Config_DisplayConfig_DisplayUnits_IMPERIAL) ? "mi" : "km");
    }

    // Draw compass circle
    display->drawCircle(compassX, compassY, compassDiam / 2);

    // Undo color-inversion, if set prior to drawing header
    // Unsure of expected behavior? For now: copy drawNodeInfo
    if (config.display.displaymode != meshtastic_Config_DisplayConfig_DisplayMode_INVERTED) {
        display->setColor(BLACK);
    }
    graphics::NodeListRenderer::drawColumns(display, x, y, fields);
}
#endif