#include <Arduino.h>


#include "espnow.h"
#include "keymap.h"


#include <Adafruit_NeoPixel.h>
#ifdef __AVR__
  #include <avr/power.h>
#endif



#ifdef PIN_NEOPIXEL
Adafruit_NeoPixel statuspixel(1, PIN_NEOPIXEL, NEO_GRB + NEO_KHZ800);
#endif

#ifdef PIXELPIN
Adafruit_NeoPixel pixel(NUMPIXELS, PIXELPIN, NEO_GRB + NEO_KHZ800); //pixels for input channels
#endif






#ifndef ARDUINO_USB_MODE
#error This ESP32 SoC has no Native USB interface
#elif ARDUINO_USB_MODE == 1
#warning This sketch should be used when USB is in OTG mode
#endif

#include "USB.h"
#include "USBHIDKeyboard.h"
USBHIDKeyboard Keyboard;

#define NUM_CONTROLLERS 4
#define NUM_CONTROLLERS_WIRED 4

uint32_t controllercolors[]={\
statuspixel.Color(255, 0,0),\
statuspixel.Color(0, 0,255),\
statuspixel.Color(255,255, 0),\
statuspixel.Color(100, 100,100)\
};

uint8_t keyassignments[NUM_CONTROLLERS][INPUT_SIZE];

uint8_t keyassignmentsWired[NUM_CONTROLLERS_WIRED];
bool in_last_wired[NUM_CONTROLLERS_WIRED];

void setup() {
  Serial.begin(115200);

  pinMode(PIN_IN1,INPUT_PULLUP);
  pinMode(PIN_IN2,INPUT_PULLUP);
  pinMode(PIN_IN3,INPUT_PULLUP);
  pinMode(PIN_IN4,INPUT_PULLUP);
  keyassignmentsWired[0]=HID_KEY_V;
  keyassignmentsWired[1]=HID_KEY_B;
  keyassignmentsWired[2]=HID_KEY_N;
  keyassignmentsWired[3]=HID_KEY_M;

  keyassignments[0][KEY_LEFT]=HID_KEY_A;
  keyassignments[0][KEY_RIGHT]=HID_KEY_D;
  keyassignments[0][KEY_UP]=HID_KEY_W;
  keyassignments[0][KEY_DOWN]=HID_KEY_S;
  keyassignments[0][KEY_A]=HID_KEY_Q;
  keyassignments[0][KEY_B]=HID_KEY_E;

  keyassignments[1][KEY_LEFT]=HID_KEY_J;
  keyassignments[1][KEY_RIGHT]=HID_KEY_L;
  keyassignments[1][KEY_UP]=HID_KEY_I;
  keyassignments[1][KEY_DOWN]=HID_KEY_K;
  keyassignments[1][KEY_A]=HID_KEY_U;
  keyassignments[1][KEY_B]=HID_KEY_O;

  keyassignments[2][KEY_LEFT]=HID_KEY_F;
  keyassignments[2][KEY_RIGHT]=HID_KEY_H;
  keyassignments[2][KEY_UP]=HID_KEY_T;
  keyassignments[2][KEY_DOWN]=HID_KEY_G;
  keyassignments[2][KEY_A]=HID_KEY_R;
  keyassignments[2][KEY_B]=HID_KEY_Z;

  keyassignments[3][KEY_LEFT]=HID_KEY_ARROW_LEFT;
  keyassignments[3][KEY_RIGHT]=HID_KEY_ARROW_RIGHT;
  keyassignments[3][KEY_UP]=HID_KEY_ARROW_UP;
  keyassignments[3][KEY_DOWN]=HID_KEY_ARROW_DOWN;
  keyassignments[3][KEY_A]=HID_KEY_CONTROL_LEFT;
  keyassignments[3][KEY_B]=HID_KEY_SHIFT_LEFT;

  initESPNOW();

  #if defined(NEOPIXEL_POWER)
    // If this board has a power control pin, we must set it to output and high
    // in order to enable the NeoPixels. We put this in an #if defined so it can
    // be reused for other boards without compilation errors
    pinMode(NEOPIXEL_POWER, OUTPUT);
    digitalWrite(NEOPIXEL_POWER, HIGH);
  #endif
  #ifdef PIN_NEOPIXEL
    statuspixel.begin();
    statuspixel.setBrightness(100); // not so bright
    statuspixel.clear();

    //Flash colors for debug
    statuspixel.setPixelColor(0, statuspixel.Color(255, 0,0));
    statuspixel.show();
    delay(100);
    statuspixel.setPixelColor(0, statuspixel.Color(0, 255,0));
    statuspixel.show();
    delay(100);
    statuspixel.setPixelColor(0, statuspixel.Color(0, 0,255));
    statuspixel.show();
    delay(100);
    statuspixel.clear();
    statuspixel.show();
    
  #endif

  #ifdef PIXELPIN
    pixel.begin();
    pixel.setBrightness(100); // not so bright
    pixel.clear();

    //Flash colors for debug
    for (uint8_t i=0;i<NUMPIXELS;i++) {
      pixel.setPixelColor(i, pixel.Color(255, 255,255));
      pixel.show();
      delay(100);
      pixel.clear();
    }
    pixel.clear();
    pixel.show();
    
  #endif


  Serial.println("Started");

  Keyboard.begin();
  bool usbresult=USB.begin();
  if (!usbresult) {
    for (uint8_t i=0;i<5;i++) {
      statuspixel.setPixelColor(0, statuspixel.Color(255, 100,0));
      statuspixel.show();
      delay(100);
      statuspixel.clear();
      statuspixel.show();
      delay(100);
    }
  }else{
    statuspixel.setPixelColor(0, statuspixel.Color(0, 255,0));
    statuspixel.show();
    delay(500);
  }
  
}



void loop() {
  unsigned long loopmillis=millis();

  bool in[NUM_CONTROLLERS_WIRED];
  in[0]=digitalRead(PIN_IN1);
  in[1]=digitalRead(PIN_IN2);
  in[2]=digitalRead(PIN_IN3);
  in[3]=digitalRead(PIN_IN4);
  static unsigned long last_in_time[NUM_CONTROLLERS_WIRED];

  for (uint8_t i=0;i<NUM_CONTROLLERS_WIRED;i++) {
    if (in[i]!=in_last_wired[i] && loopmillis-last_in_time[i]>=DEBOUNCETIME_WIRED) { //changed?
      if (!in[i]) {
        Keyboard.pressRaw(keyassignmentsWired[i]);
        pixel.setPixelColor(i, pixel.Color(0,0,0));
      }else{
        Keyboard.releaseRaw(keyassignmentsWired[i]);
        pixel.setPixelColor(i, controllercolors[i]);
      }
      pixel.show();
      last_in_time[i]=millis();
      in_last_wired[i]=in[i];
    }
    
  }

}

void processCommand(struct_message espnowdata) { //espnow data from remote



  Serial.println(millis());
  Serial.print("ID="); Serial.println(espnowdata.id);
  Serial.print("Action="); Serial.println(espnowdata.action);
  Serial.print("Input="); Serial.println(espnowdata.input);
  Serial.println();
#ifdef WIRELESS_ENABLED
  if (espnowdata.id > NUM_CONTROLLERS-1){
    Serial.print("ID out of range");
    return;
  }

  
  if (espnowdata.action==PRESS){
    statuspixel.setPixelColor(0, controllercolors[espnowdata.id]); //change status led when packet arrives
    Keyboard.pressRaw(keyassignments[espnowdata.id][espnowdata.input]);
  }else if (espnowdata.action==RELEASE) {
    statuspixel.setPixelColor(0, 0); //change status led when packet arrives
    Keyboard.releaseRaw(keyassignments[espnowdata.id][espnowdata.input]);
  }
  statuspixel.show();
#endif
}
