#pragma once


char buffer[128];
void printState(PsIoBuffer* b){
    sprintf(buffer, "State: %d, %d, %d, %d, %d, %d, %d, %d, %d         \r", 
        (int)b->data[0], (int)b->data[1], (int)b->data[2], (int)b->data[3], 
        (int)b->data[4], (int)b->data[5], (int)b->data[6], (int)b->data[7], 
        (int)b->data[8] );
    print(buffer);
}


float intToFloatAxisValue(std::uint8_t value) {
    int hysteresis = 50;
    int hysteresisLow = (255 - hysteresis) / 2;
    int hysteresisHi = (255 + hysteresis) / 2;

    if(value < hysteresisHi && value > hysteresisLow){
        return 0.0f;
    }

    if(value <= hysteresisLow){
        return -(1.0f - (value / float(hysteresisLow)));
    }

    if(value >= hysteresisHi){
        return (value - hysteresisHi) / (float)(255 - hysteresisHi);
    }

    return 0.0f;
}


struct DataMask {
    struct {
        uint8_t b0;
        uint8_t b1;
        uint8_t b2;
    } value;

    constexpr DataMask operator | (const DataMask& other) const{
        return {{value.b0 | other.value.b0, value.b1 | other.value.b1, value.b2 | other.value.b2,}};
    }

    static constexpr DataMask digitalB0() { return {{0x01, 0x00, 0x00}}; }
    static constexpr DataMask digitalB1() { return {{0x02, 0x00, 0x00}}; }
    static constexpr DataMask allDigital() { return digitalB0() | digitalB1();}

    static constexpr DataMask analogJoyRx() { return {{0x04, 0x00, 0x00}}; }
    static constexpr DataMask analogJoyRy() { return {{0x08, 0x00, 0x00}}; }
    static constexpr DataMask analogJoyLx() { return {{0x10, 0x00, 0x00}}; }
    static constexpr DataMask analogJoyLy() { return {{0x20, 0x00, 0x00}}; }

    static constexpr DataMask leftAnalogJoy() { return analogJoyLx() | analogJoyLy(); }
    static constexpr DataMask rightAnalogJoy() { return analogJoyRx() | analogJoyRy(); }
    static constexpr DataMask allAnalogJoy() { return leftAnalogJoy() | rightAnalogJoy(); }

    static constexpr DataMask pressureR() { return {{0x40, 0x00, 0x00}}; }
    static constexpr DataMask pressureL() { return {{0x80, 0x00, 0x00}}; }
    static constexpr DataMask pressureU() { return {{0x00, 0x01, 0x00}}; }
    static constexpr DataMask pressureD() { return {{0x00, 0x02, 0x00}}; }

    static constexpr DataMask pressureTriangle() { return {{0x00, 0x04, 0x00}}; }
    static constexpr DataMask pressureCircle() { return {{0x00, 0x08, 0x00}}; }
    static constexpr DataMask pressureCross() { return {{0x00, 0x10, 0x00}}; }
    static constexpr DataMask pressureSquare() { return {{0x00, 0x20, 0x00}}; }

    static constexpr DataMask pressureL1() { return {{0x00, 0x40, 0x00}}; }
    static constexpr DataMask pressureR1() { return {{0x00, 0x80, 0x00}}; }

    static constexpr DataMask pressureL2() { return {{0x00, 0x00, 0x01}}; }
    static constexpr DataMask pressureR2() { return {{0x00, 0x00, 0x02}}; }

    static constexpr DataMask allPressure() {
        return
                pressureR() | pressureL() | pressureU() | pressureD() |
                pressureTriangle() | pressureCircle() | pressureCross() | pressureSquare() |
                pressureL1() | pressureR1() | pressureL2() | pressureR2();
    }

    static constexpr DataMask allDigitalAndAnalogJoy() {
        return allDigital() | allAnalogJoy();
    }

    static constexpr DataMask all() {
        return allDigitalAndAnalogJoy() | allPressure();
    }

    static constexpr DataMask none() {
        return ~(allDigitalAndAnalogJoy() | allPressure());
    }

    constexpr DataMask operator ~() {
        return {{static_cast<uint8_t>(~value.b0 & 0xff), static_cast<uint8_t>(~value.b1 & 0xff), static_cast<uint8_t>(~value.b2 & 0x03)}};
    }

};


extern "C" {
    extern SPI_HandleTypeDef hspi1;
}

__STATIC_INLINE void DWT_Delay_us(volatile uint32_t microseconds){
    /* Go to number of cycles for system */
    microseconds *= (HAL_RCC_GetSysClockFreq() / 1000000);

    DWT->CYCCNT = 0;
    /* Delay till end */
    while (DWT->CYCCNT < microseconds);
}

void byteDelay() {
    DWT_Delay_us(28);
}

struct PollResponse {
    uint8_t dummy;
    uint8_t id;
    uint8_t padID;
    uint8_t buttons0;
    uint8_t buttons1;
    uint8_t analogRX;
    uint8_t analogRY;
    uint8_t analogLX;
    uint8_t analogLY;

    struct {
        uint8_t right;
        uint8_t left;
        uint8_t up;
        uint8_t down;
        uint8_t triangle;
        uint8_t circle;
        uint8_t cross;
        uint8_t square;
        uint8_t l1;
        uint8_t r1;
        uint8_t l2;
        uint8_t r2;

    }pressure;


    bool hasAnalogJoyData() {
        return (id & 0xf) >=3;
    }

    bool select() {
        return (buttons0 & (1 << 0)) == 0;
    }

    bool l3() {
        return (buttons0 & (1 << 1)) == 0;
    }
    bool r3() {
        return (buttons0 & (1 << 2)) == 0;
    }
    bool start() {
        return (buttons0 & (1 << 3)) == 0;
    }
    bool up() {
        return (buttons0 & (1 << 4)) == 0;
    }
    bool right() {
        return (buttons0 & (1 << 5)) == 0;
    }
    bool down() {
        return (buttons0 & (1 << 6)) == 0;
    }
    bool left() {
        return (buttons0 & (1 << 7)) == 0;
    }


    bool l2() {
        return (buttons1 & (1 << 0)) == 0;
    }
    bool r2() {
        return (buttons1 & (1 << 1)) == 0;
    }
    bool l1() {
        return (buttons1 & (1 << 2)) == 0;
    }
    bool r1() {
        return (buttons1 & (1 << 3)) == 0;
    }
    bool triangle() {
        return (buttons1 & (1 << 4)) == 0;
    }
    bool circle() {
        return (buttons1 & (1 << 5)) == 0;
    }
    bool cross() {
        return (buttons1 & (1 << 6)) == 0;
    }
    bool square() {
        return (buttons1 & (1 << 7)) == 0;
    }
};


struct PsIoStm32HalSpiPhy : public PsIoPhy {
    PsIoStm32HalSpiPhy() {
        end();
    }
    bool begin() override {
        HAL_GPIO_WritePin(PAD_CS_GPIO_Port, PAD_CS_Pin, GPIO_PIN_RESET);
        byteDelay();
        return true;
    }
    bool shift(uint8_t txData, uint8_t* rxData) override {
        
        HAL_SPI_TransmitReceive(&hspi1, &txData, rxData, 1, HAL_MAX_DELAY);
		byteDelay();
        return true;
    }
    void end() override {
        HAL_GPIO_WritePin(PAD_CS_GPIO_Port, PAD_CS_Pin, GPIO_PIN_SET);
        byteDelay();
    }
};