#pragma once
#include "olcPixelGameEngine.h"
#include "SMX.h"
#include <assert.h>

using namespace olc;

class PGEX_SMX : public PGEX{
public:
    PGEX_SMX():PGEX(true){
        
    };
    //UP is 0x2
    //RIGHT is 0x20
    //DOWN is 0x80
    //LEFT is 0x08
    //Add these bytes together to get the combination (all 4 held down is 0xAA
    HWButton GetPanel(Key k,int pad=0)const{
        return panelState[k-UP][pad];
    };
    void EnableLogMessages(bool enabled){
        logMessages=enabled;
    }
private:
    static bool logMessages;
    HWButton internal_panelState[4][2];
    HWButton panelState[4][2]; //We store the 4 main panel states, similar to how PGE does.
    double totalElapsedTime{0.};
    double nextLightUpdate{0.};
    //SMX "screen" is 12x21
    //Each panel is 4x7
    static void SMXStateChangedCallback(int pad, SMXUpdateCallbackReason reason, void *pUser)
    {
        PGEX_SMX *pSelf = (PGEX_SMX*) pUser;
        pSelf->SMXStateChanged( pad, reason );
    }

    static void SMXLogCallback(const char *log)
    {
        if(logMessages){
            printf("-> %s\n", log);
        }
    }

    //The key enums are UP, DOWN, LEFT, RIGHT.
    void SMXStateChanged(int pad, SMXUpdateCallbackReason reason)
    {
        if(logMessages){
            printf("Device %i state changed: %04x\n", pad, SMX_GetInputState(pad));
            printf("Device %i state changed: %04x\n", 1, SMX_GetInputState(1));
        }
        int16_t state(SMX_GetInputState(0));
        if(state&0x2&&!internal_panelState[UP-UP][0].bPressed){
            internal_panelState[UP-UP][0]={true,false,true};
        }else
        if(!(state&0x2)&&!internal_panelState[UP-UP][0].bReleased){
            internal_panelState[UP-UP][0]={false,true,false};
        }
        if(state&0x20&&!internal_panelState[RIGHT-UP][0].bPressed){
            internal_panelState[RIGHT-UP][0]={true,false,true};
        }else
        if(!(state&0x20)&&!internal_panelState[RIGHT-UP][0].bReleased){
            internal_panelState[RIGHT-UP][0]={false,true,false};
        }
        if(state&0x80&&!internal_panelState[DOWN-UP][0].bPressed){
            internal_panelState[DOWN-UP][0]={true,false,true};
        }else
        if(!(state&0x80)&&!internal_panelState[DOWN-UP][0].bReleased){
            internal_panelState[DOWN-UP][0]={false,true,false};
        }
        if(state&0x8&&!internal_panelState[LEFT-UP][0].bPressed){
            internal_panelState[LEFT-UP][0]={true,false,true};
        }else
        if(!(state&0x8)&&!internal_panelState[LEFT-UP][0].bReleased){
            internal_panelState[LEFT-UP][0]={false,true,false};
        }
        state=SMX_GetInputState(1);
        if(state&0x2&&!internal_panelState[UP-UP][1].bPressed){
            internal_panelState[UP-UP][1]={true,false,true};
        }else
        if(!(state&0x2)&&!internal_panelState[UP-UP][1].bReleased){
            internal_panelState[UP-UP][1]={false,true,false};
        }
        if(state&0x20&&!internal_panelState[RIGHT-UP][1].bPressed){
            internal_panelState[RIGHT-UP][1]={true,false,true};
        }else
        if(!(state&0x20)&&!internal_panelState[RIGHT-UP][1].bReleased){
            internal_panelState[RIGHT-UP][1]={false,true,false};
        }
        if(state&0x80&&!internal_panelState[DOWN-UP][1].bPressed){
            internal_panelState[DOWN-UP][1]={true,false,true};
        }else
        if(!(state&0x80)&&!internal_panelState[DOWN-UP][1].bReleased){
            internal_panelState[DOWN-UP][1]={false,true,false};
        }
        if(state&0x8&&!internal_panelState[LEFT-UP][1].bPressed){
            internal_panelState[LEFT-UP][1]={true,false,true};
        }else
        if(!(state&0x8)&&!internal_panelState[LEFT-UP][1].bReleased){
            internal_panelState[LEFT-UP][1]={false,true,false};
        }
    }

    virtual void OnAfterUserCreate()override{
        SMX_Start( SMXStateChangedCallback, this );
    };

    virtual bool OnBeforeUserUpdate(float& fElapsedTime){
        //We don't want the panel states changing in the middle of a frame, since the input polling is on a separate thread.
        //Copy over the internal button states to the current panel state.
        for(int pad=0;pad<2;pad++){
            for(int i=0;i<4;i++){
                panelState[i][pad]=internal_panelState[i][pad];
                internal_panelState[i][pad].bPressed=false;
                internal_panelState[i][pad].bReleased=false;
            }
        }
        return false;
    }

    virtual void OnAfterUserUpdate(float fElapsedTime)override{
        double previousTotalElapsedTime{totalElapsedTime};
        totalElapsedTime=fmod(totalElapsedTime+fElapsedTime,10000.);
        if(previousTotalElapsedTime>totalElapsedTime){
            //We are in the past, so reset lastLightUpdate to keep lights running.
            nextLightUpdate=totalElapsedTime;
        }
        if(nextLightUpdate>totalElapsedTime)return; //Ignore light updates if not enough time has passed. We can only send light updates at 30 FPS.
        for(int pad=0;pad<2;pad++){
            for(int i=0;i<4;i++){
                panelState[i][pad].bPressed=false;
                panelState[i][pad].bReleased=false;
            }
        }
        std::string lightData;
        //The light data for an SMX dance pad is outlined in the docs but the code used to transform the PGE's pixels to SMX pad's lights will be annotated here.
        //Both pads receive data sequentially, and for now we can mimic the data on both ends.
        for (int pad=0;pad<2;pad++){
            for(int i=0;i<9;i++){
                int row=0;
                for(int y=i/3*7;y<i/3*7+7;y+=2){
                    int col=0;
                    for(int x=i%3*4;x<i%3*4+4;x++){
                        if(row%2==1&&col%4==0){
                            col++;
                            continue;
                        }
                        Pixel p(pge->GetDrawTarget()->GetPixel(x+12*pad,y));
                        lightData.append(1,p.r);
                        lightData.append(1,p.g);
                        lightData.append(1,p.b);
                        col++;
                    }
                    row+=2;
                }   
                row=1;
                for(int y=i/3*7+1;y<i/3*7+7;y+=2){
                    int col=0;
                    for(int x=i%3*4;x<i%3*4+4;x++){
                        if(row%2==1&&col%4==0){
                            col++;
                            continue;
                        }
                        Pixel p(pge->GetDrawTarget()->GetPixel(x+12*pad,y));
                        lightData.append(1,p.r);
                        lightData.append(1,p.g);
                        lightData.append(1,p.b);
                        col++;
                    }
                    row+=2;
                }   
            }
        }
        if(lightData.size()!=1350)throw;
        nextLightUpdate=totalElapsedTime+1/30.f;
        SMX_SetLights2( lightData.data(), lightData.size() );
    }
};

bool PGEX_SMX::logMessages=false;