#include <sta/debug/spatz.hpp>

#ifdef STA_SPATZ_ENABLED

#ifndef STA_INIT_SPATZ_ID
#	error "The ID used to communicate SPATZ initialization was not defined."
#endif // STA_INIT_SPATZ_ID

#include <usart.h>
#include <sta/time.hpp>

namespace sta
{
    namespace spatz
    {
        Mutex * mutex_ 		= nullptr;
        Event * signal_ 	= nullptr;

        void init(sta::Mutex * mutex, Event * signal)
        {
            mutex_ = mutex;
            signal_ = signal;

            uint8_t msg = 0xFF;

            // Wait until SPATZ sends the init byte.
            while (msg != STA_INIT_SPATZ_ID)
            {
            	Debug->read(&msg, 1);
            }
        }

        void tick()
        {
        	mutex_->acquire();
        	Debug->println('~');
        	mutex_->release();
        }

        void request(uint8_t id, uint8_t * buffer, size_t length)
        {
            mutex_->acquire();

			Debug->printf("%d", id);
			Debug->read(buffer, length);

			mutex_->release();
        }

        void request(uint8_t id, float * buffer, size_t length)
        {
        	mutex_->acquire();

            HAL_UART_Receive_IT(&huart1, reinterpret_cast<uint8_t*>(buffer), length * sizeof(float));
            Debug->printf("%d", id);
            signal_->wait(0x01, 100);

            mutex_->release();
        }

        void request(uint8_t id, double * buffer, size_t length)
        {
        	mutex_->acquire();

			Debug->printf("%d", id);
			Debug->read(buffer, length);

			mutex_->release();
        }

        void notify(uint8_t id)
        {
			Debug->printf("%d", id);
        }
    } // namespace spatz    
} // namespace sta


void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
{
	if (sta::spatz::signal_ != nullptr)
	{
		sta::spatz::signal_->set(0x01);
		sta::spatz::signal_->clear(0x01);
	}
}

#else

namespace sta
{
	namespace spatz
	{
		void init(sta::Mutex * mutex, Event * signal)
	    {
	        // Nothing to do here.
	    }
	}  // namespace spatz
} // namespace sta

#endif // STA_SPATZ_ENABLED