/*
 * shared.cpp
 *
 *  Created on: Jun 17, 2024
 *      Author: Dario
 */

#include <shared.hpp>
#include <sta/tacos.hpp>

#include <sta/rtos/mutex.hpp>
#include <sta/rtos/event.hpp>
#include <sta/rtos/timer.hpp>
#include <sta/rtos/sharedmem.hpp>

#include <sta/time.hpp>

namespace rres
{
    sta::RtosEvent *signal;
    sta::RtosSharedMem<LoggingData> *memory;
    sta::RtosTimer *sync_10Hz;

    bool _10Hz_tick = true;

    void init()
    {
        signal = new sta::RtosEvent();
        memory = new sta::RtosSharedMem<LoggingData>(100);

        sync_10Hz = new sta::RtosTimer([](void *)
                                       { 
            if (_10Hz_tick)
            {
                signal->set(TICK_10HZ);
            }
            else
            {
                signal->set(TOCK_10HZ);
            }

            _10Hz_tick = !_10Hz_tick; }, NULL, true);
        sync_10Hz->start(50);
    }

    uint32_t waitForAny(uint32_t dataTypes, uint32_t timeout /* = osWaitForever */)
    {
        uint32_t flags = signal->wait(dataTypes, timeout);
        signal->clear(dataTypes);

        return flags;
    }

    uint32_t waitForAll(uint32_t dataTypes, uint32_t timeout /* = osWaitForever */)
    {
        uint32_t received = 0x00;
        uint32_t startTime = sta::timeMs();

        do
        {
            // Wait for the remaining flags and set the remaining time as the timeout.
            received |= waitForAny(dataTypes & ~received, timeout - (sta::timeMs() - startTime));
        } while (received != dataTypes && sta::timeMs() - startTime < timeout);

        signal->clear(received);

        return received;
    }

    void setSenseFire(uint8_t sense_fire)
    {
        LoggingData data = memory->read();
        data.sense_fire = sense_fire;
        memory->write(data);
    }

    void setSenseADC(uint16_t sense_adc[6])
    {
        LoggingData data = memory->read();
        for (int i = 0; i < 6; i++)
        {
            data.sense_adc[i] = sense_adc[i];
        }
        memory->write(data);
    }

    void setFired(uint8_t fired)
    {
        LoggingData data = memory->read();
        data.fired = fired;
        memory->write(data);
    }

    LoggingData getData()
    {
        return memory->read();
    }

    void startLogging()
    {
        signal->set(START_LOGGING);
    }

    void dumpFlash()
    {
        signal->set(DUMP_FLASH);
    }

    void clearFlash()
    {
        signal->set(CLEAR_FLASH);
    }
} // namespace rres

#ifdef STA_TACOS_CAN_BUS_ENABLED
namespace sta
{
    namespace tacos
    {
        bool handleSysMessage(CanMsgHeader &header, uint8_t *payload)
        {
            // STA_DEBUG_PRINTF("> CAN ID recved: %d", header.sid);
            switch (header.sid)
            {
            // State transition message
            case STA_TACOS_CAN_BUS_SYS_MSG_ID:
                // First byte of payload is the origin state, second byte is the destination state
                tacos::forceState(payload[0], payload[1], 0, true);

                // Send confirmation message
                CanSysMsg msg;
                msg.header.payloadLength = 2;
                msg.header.sid = MODULE_STATE_CONFIRM_CAN_ID;
                msg.header.eid = 0;
                msg.header.format = 0;
                msg.payload[0] = MODULE_RRES;
                msg.payload[1] = payload[1];

                tacos::queueCanBusMsg(msg, 0);

                return true;

            case MODULE_SW_RESET_CAN_ID:
                if (payload[0] == MODULE_RRES)
                {
                    HAL_NVIC_SystemReset();
                }
                return true; // Least useless return statement ever

            // Flash Operations
            case FLASH_OPERATIONS_CAN_ID:
                if (payload[0] == 0x01)
                    rres::clearFlash();
                else if (payload[0] == 0x02)
                    rres::dumpFlash();
                return true;

            default:
                return false;
            }
        }
    }
}
#endif // STA_TACOS_CAN_BUS_ENABLED‚