#include "../../internal-include/periphery_internal.hpp"
#include <PSX/Periphery/periphery.hpp>
#include <PSX/System/syscalls.hpp>

namespace JabyEngine {
    namespace Periphery {
        // Controllers are checked every alternating frame
        static uint8_t cur_controller_port = 0;
        RawController  controller[PortCount][DeviceCount];

        struct ControllerHelper {
            static bool is_config(const RawController &cont) {
                return (static_cast<uint8_t>(cont.header.state) & 0xF);
            }

            static void advance_config(RawController &cont) {
                cont.header.state = static_cast<RawController::State>((static_cast<uint32_t>(cont.header.state) << 1));
            }

            static uint8_t* raw_device_data(RawController &cont) {
                return reinterpret_cast<uint8_t*>(&cont.button.currentState);
            }
        };

        static void set_config_command(RawController::State state, uint8_t (&header)[3], uint8_t (&data)[6]) {
            static constexpr uint32_t CMDIDX = 1;

            switch(state) {
                case RawController::State::EnterConfigMode:
                    static constexpr uint8_t EnterCFGModeCMD = 0x1;

                    header[CMDIDX] = 0x43;
                    data[0]        = EnterCFGModeCMD;
                break;

                case RawController::State::LockAnalog:
                    static constexpr uint8_t valID = 0;
                    static constexpr uint8_t selID = 1;

                    header[CMDIDX] = 0x44;
                    data[valID]    = static_cast<uint8_t>(LED::State::On);
                    data[selID]    = static_cast<uint8_t>(LED::Lock::On);
                break;

                case RawController::State::UnlockRumble:
                    header[CMDIDX] = 0x4D;

                    data[1] = 0x1;
                    for(int32_t n = 2; n < 6; n++) {
                        data[n] = 0xFF;
                    }
                break;

                case RawController::State::ExitConfigMode:
                    header[CMDIDX] = 0x43;
                break;
            }
        }

        static size_t send_data(const uint8_t* header, uint8_t* headerResponse, const uint8_t* data, uint8_t* response) {
            const uint8_t *src  = header;
            uint8_t       *dst  = headerResponse;
            size_t         size = 3;

            for(size_t n = 0; n < size; n++) {
                Periphery::send_byte(*src);
                Periphery::acknowledge();
                *dst = Periphery::read_byte();

                if(n == 2) {
                    const uint8_t id = headerResponse[1];
                    size += (id == 0xFF) ? 0 : ((id & 0xF) << 1);

                    src = data;
                    dst = response;
                }

                else {
                    src++;
                    dst++;
                }

                busy_loop(15);
            }
            return size;
        }

        void query_controller() {
            static constexpr auto TypeIDX = 1;

            SysCall::EnterCriticalSection();
                Periphery::connect_to(cur_controller_port);
                for(uint32_t id = 0; id < Periphery::DeviceCount; id++) {
                    auto &cur_controller = controller[cur_controller_port][id];

                    uint8_t header[] = {static_cast<uint8_t>((id + 1)), 0x42, 0x0};
                    uint8_t data[]   = {cur_controller.header.rumble0, cur_controller.header.rumble1, 0x0, 0x0, 0x0, 0x0};

                    // Can this move to the if??
                    set_config_command(cur_controller.header.state, header, data);
                    if(ControllerHelper::is_config(cur_controller)) {    
                        send_data(header, header, data, data);
                        ControllerHelper::advance_config(cur_controller);
                    }

                    else {
                        cur_controller.button.exchange_state();

                        send_data(header, header, data, ControllerHelper::raw_device_data(cur_controller));
                        const bool isValidController = (header[TypeIDX] != 0xFF);

                        if(header[TypeIDX] != cur_controller.header.type) {
                            cur_controller.header.type  = header[TypeIDX];
                            cur_controller.header.state = isValidController ? RawController::State::EnterConfigMode : RawController::State::Disconnected;

                            /*if(!isValidController)
                            {
                                printf("Disconnected!\n");
                            }

                            else
                            {
                                printf("ID: 0x%02X 0x%04X\n", header[TypeIDX], contData.button);
                            }*/
                        }
                    }
                }
                Periphery::close_connection();

                if(Configuration::Periphery::include_portB()) {
                    cur_controller_port ^= 0x1;
                }
            SysCall::ExitCriticalSection();
        }
    }
}