/*
 * tacos.cpp
 *
 *  Created on: Jan 2, 2024
 *      Author: Dario
 */

#include <sta/tacos.hpp>

namespace sta
{
    namespace tacos
    {
		uint16_t getState()
        {
            return Statemachine::instance()->getCurrentState();
        }

		void setState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */, bool force /* = false */)
        {
            Statemachine::instance()->requestStateTransition(from, to, lockout, force);
        }

		void setStateTimed(uint32_t from, uint32_t to, uint32_t millis, uint32_t lockout /* = 0 */)
        {
            Statemachine::instance()->requestTimedStateTransition(from, to, millis, lockout);
        }

#ifdef STA_TACOS_CAN_BUS_ENABLED
        bool queueCanBusMsg(CanSysMsg & msg, uint32_t timeout){
			return CanBus::instance()->queueCanBusMsg(msg, timeout);
		}

        bool publishState(uint32_t from, uint32_t to, uint32_t lockout /* = 0 */){
            CanSysMsg msg;
            msg.header.sid = STA_TACOS_CAN_BUS_SYS_MSG_ID;
            msg.header.payloadLength = 2;
            msg.payload[0] = from;
            msg.payload[1] = to;

            msg.header.eid = 0;
            msg.header.format = 0;

            return CanBus::instance()->queueCanBusMsg(msg, lockout);
        }
#endif // STA_TACOS_CAN_BUS_ENABLED
    } // namespace tacos
    
} // namespace sta