// LED_BUILTIN is connected to pin 25 of the RP2040 chip.

// POWER OPTIONS FOR DIGITAL OUTPUT ARE:
// - OUTPUT, OUTPUT_2MA, OUTPUT_4MA, OUTPUT_8MA, OUTPUT_12MA

#include "Arduino.h"
#include "stdio.h"
#include "Toggle.h"
//#include "pico/stdlib.h"
//#include "Adafruit_TinyUSB.h"

#define ELEMENTS(x) (sizeof(x)/sizeof(x[0]))

#define u_long unsigned long
#define setPin digitalWrite
#define BAUD 115200

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/* OPTIONS */
bool debugMode = false;

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
//typedef enum : int {OFF, ON} states;

typedef enum : int {
  REL   = 0,
  PRESS = 1,
  HELD  = 2
} States;

enum : int {
  LED       =   LED_BUILTIN,
  READY_PIN =   10, 
  BULBS     =   16,
  SOLENOID  =   17,
  RUMBLE    =   18,
  CLICK     =   19, 
  BTN       =   20,
  TRIGGER   =   21,
  ENABLE    =   22
};

typedef struct { 
  const char  *label;
  int          pin; 
} PINS;

PINS outPins[] = {
  {"BULBS",    BULBS},
  {"SOLENOID", SOLENOID},
  {"RUMBLE",   RUMBLE},
  {"CLICK",    CLICK},
  {"LED",      LED}
};

PINS inPins[] = {
  {"BTN",     BTN},
  {"TRIGGER", TRIGGER},
  {"ENABLE",  ENABLE}
};

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */



unsigned long startTime = 0;

Toggle trigger(TRIGGER);
Toggle gun_en(ENABLE);
Toggle btn(BTN);

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */


template<typename T, size_t N>
void setPins(const T (&arr)[N], int state) {
  for(int i = 0; i < N; ++i) setPin(arr[i], state);
}

bool inRange(int min, int max = 32767, int val = elapsed()) {
  return min < val && val <= max; 
}

/*
template<typename T>
bool inRange(const T &val, int min, int max) {
  return min < val && val <= max; 
}
*/

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

//unsigned long elapsed() { 
//  return (millis() - startTime); 
//} 

uint16_t elapsed() { 
  return (uint16_t)(millis()-startTime); 
} 

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void null(){ ; }

void whileWait(uint16_t limit, void (*)());
void whileWait(uint16_t limit, void (*function)()) {
  startTime = millis();
  while (elapsed() < limit) (*function)();
}
void whileWait(uint16_t limit) {
  whileWait(limit, null);
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */


void setup() {
  byte i;
  
  for(i=0; i<ELEMENTS(inPins); ++i) 
    pinMode(inPins[i].pin, INPUT_PULLUP);
    
  for(i=0; i<ELEMENTS(outPins); ++i) 
    pinMode(outPins[i].pin, OUTPUT_12MA);
    
  pinMode(READY_PIN,        OUTPUT_12MA);
    
  /* ---------------------------------------------- */
  //Serial1.ignoreFlowControl(true); 
  Serial1.begin(BAUD);
  //while (!Serial1 && !BOOTSEL) setPin(LED, HIGH), delay(30), setPin(LED, LOW), delay(30);
  setPin(LED, LOW);
  Serial1.println("started");
  
  /* ---------------------------------------------- */
  setPin(READY_PIN, HIGH); 
 
  trigger.begin(TRIGGER);
  gun_en.begin(ENABLE);
  btn.begin(BTN);
}
/* END: setup() */

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
bool retrigger = true;
bool solenoidDone = true;

bool triggered = false;
byte gunBusy = 0;

uint16_t recoilTime = 80;
uint16_t recoilRecover = 40;
uint16_t triggerTime = ((recoilTime * 2)+recoilRecover);
uint16_t retrig_ms = (recoilTime + recoilRecover);

uint16_t retrig_delay = 100;

int fireState = -1;

unsigned long solenoidTimer   = 0,
              solenoidCounter = 0,
              solenoidLimit   = 10000;

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void Fire(int mode) {
  startTime = millis();
  setPin(SOLENOID, HIGH), setPin(BULBS, HIGH), setPin(LED, LOW);
  
  if (mode == 1) {
    while (elapsed() < triggerTime) {
      if ( elapsed() > 30)            setPin(BULBS, LOW); 
      if ( elapsed() > 50)            setPin(RUMBLE, HIGH);
      if ( elapsed() > recoilTime)    setPin(SOLENOID, LOW); 
      if (elapsed() > recoilTime*2) {   setPin(RUMBLE, LOW); 
        if (trigger.onPress()) break; //if (trigger.isPressed()) break;
      }
      trigger.poll();
    }
    setPin(SOLENOID, LOW), setPin(RUMBLE, LOW), setPin(BULBS, LOW),setPin(LED, HIGH);
  } else if (mode == 2) {
    bool isReleased = false;
    while (elapsed() < triggerTime) { //while (elapsed() < (recoilTime*2)) {
      if (trigger.isReleased()) break;
      
      if ( elapsed() > 20)            setPin(BULBS, LOW);
      if ( inRange(40, recoilTime))   setPin(SOLENOID, LOW);
      if ( inRange(recoilTime, 120))  setPin(SOLENOID, HIGH); 
      if ( elapsed() > 120)           setPin(SOLENOID, LOW);
      if (elapsed() > (recoilTime*2)) break;

      trigger.poll();
    }
    setPin(SOLENOID, LOW), setPin(RUMBLE, LOW), setPin(BULBS, LOW),setPin(LED, HIGH);
  }  
  setPin(LED, HIGH);
  Serial1.printf("Time: %d \n", elapsed());
}
/* END: Fire() */


/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
//int testCount = 0;
void checkTrigger() {
  if (gunEnabled()) {
    if (trigger.onPress()) { //if (trigState() == PRESS) {
      Fire(1);
      while (trigger.isPressed()) { //while (trigState() == HELD) {
        Fire(2);
        trigger.poll(); 
      }
    }
  }
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void Blink(int pin = 25, int del = 250, bool postDelay = false) {
  setPin(LED, HIGH), setPin(pin, HIGH);
  delay(del);
  setPin(LED, LOW), setPin(pin, LOW);
  if (postDelay) delay(del);
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void clickPattern(int pattern) {
  switch (pattern) {
    case 1:
      for (int i=0; i<=10; ++i) Blink(CLICK, 100, true);
      break;
    default:
      break;
  }
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/*
            ____    _____   ____    _   _    ____ 
           |  _ \  | ____| | __ )  | | | |  / ___|
           | | | | |  _|   |  _ \  | | | | | |  _ 
           | |_| | | |___  | |_) | | |_| | | |_| |
           |____/  |_____| |____/   \___/   \____|
                                        
*/
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

/*
int retrig_check(int mode = 1) {
  trigger.poll();
  if (mode == 1) {
    if (trigger.onPress())    return 0;
    if (trigger.isPressed())  return 1;
  } else if (mode == 2) {
    if (trigger.isPressed())  return 1;
    if (trigger.isReleased()) return 0;
  }
}
*/

// 40, 100 = sweet spot?
void TestFire(int mode, int low = 40, int high = 100) {
  Serial1.printf("\nTestFire:\n  mode: %d\n  low: %d\n  high: %d\n\n",mode,low,high);
  
  if (solenoidTimer == 0) solenoidTimer = millis();
  startTime = millis();
  
  setPin(SOLENOID, HIGH), setPin(BULBS, HIGH), setPin(LED, LOW);

  while ( elapsed() <= (low/2));
  setPin(BULBS, LOW); 
  
  while ( elapsed() <= low);
  setPin(RUMBLE, HIGH);
  if (mode == 2) setPin(SOLENOID, LOW); 

  while (!trigger.onPress()) {
    while ( elapsed() <= high);
    if (mode == 1) setPin(SOLENOID, LOW);
    if (mode == 2) setPin(SOLENOID, HIGH);
    
    while ( elapsed() <= (high+low)) trigger.poll();
    setPin(RUMBLE, LOW);
    if (mode == 2) setPin(SOLENOID, LOW); 

    while ( elapsed() <= (high+low)) trigger.poll();
    setPin(RUMBLE, LOW);
    if (mode == 2) setPin(SOLENOID, LOW); 

    while ( elapsed() <= (high*2)) trigger.poll();
    setPin(LED, HIGH);
    break;
  }
  if (mode == 1) solenoidCounter += high; 
  if (mode == 2) solenoidCounter += (low*2);
  
    

  //setPin(RUMBLE, LOW);
  /*
  if (mode == 1) {
    solenoidCounter += high; 
    if (trigger.isPressed()) {
      while ( elapsed() <= ((high*2)+retrig_delay) );
    }
    //while (elapsed() <= ((high*2)+retrig_delay)) {
    //  if (trigger.onPress()) return,
    //  trigger.poll();
    //}
  }
  */

}
/* END: TestFire() */

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void debugger(int mode = 0, int del = 500) {
  bool    customDelays = true;
  int     customDel[5] = {30,80,100,60,50},
          ints[2],
          strToInt,
          activePin,
          activeIndex,
          StringCount = 0;
          
  String  activeLabel,
          cmd = "";

  
  switch(mode) {
    case 1:
      Serial1.printf("debugger:\n  mode: %d - Serial1 ignite test. (auto)\n\n", mode);
      while (1) {
        while (!trigger.onPress()) trigger.poll(); 
        TestFire(1, 40, 100);
        //trigger.poll(); 
        if (trigger.pressedFor(100)) {
          while (trigger.isPressed()) {
            TestFire(2, 40, 100);
            if (trigger.isReleased()) break;
            //trigger.poll(); 
          }
        }
        setPin(CLICK, HIGH);
        while ( (elapsed() <= 300) && !trigger.onPress()) {
          trigger.poll(); 
        }
        setPin(CLICK, LOW);
        //delay(30);
        //Blink(CLICK, 100);
      }
      break;
    case 2:
      Serial1.printf("debugger:\n  mode: %d - Serial1 ignite test.\n\n  args: 'half, recoil, onehalf, limit'\n\n", mode);
      while (1) {
        while (Serial1.available() == 0) {
          if (StringCount == ELEMENTS(ints)) {
            while (!trigger.onPress()) trigger.poll(); 
            //if (trigger.onPress()) {
              TestFire(1, ints[0],ints[1]);
              //if (trigger.isPressed()) {
                trigger.poll(); 
                while (trigger.isPressed()) {
                  TestFire(2, ints[0],ints[1]);
                  trigger.poll(); 
                  if (trigger.isReleased()) break;
                }
                
              //}
            //}
          }
        }
        if (Serial1.available() > 0) {
          StringCount = 0;
          cmd = Serial1.readStringUntil('\n');
          if (cmd == "STOP" || cmd == "OFF") break;
          
          while (cmd.length() > 0) {
            int index = cmd.indexOf(' ');
            if (index == -1) { // No space found
              ints[StringCount++] = cmd.toInt(); break;
            } else {
              ints[StringCount++] = cmd.substring(0, index).toInt();
              cmd = cmd.substring(index+1);
            }
          }
          
          if (StringCount == ELEMENTS(ints)) {
            Serial1.println("VALID INPUT");
            for (int i = 0; i < StringCount; i++) Serial1.printf("%d: %d \n", i, ints[i]);
          }
        }
        Serial1.println("\nDONE!");
      }
      break;
    case 3:
      activePin = 0;
      activeIndex = -1;
      Serial1.printf("debugger:\n  mode: %d - readSerial1.\n\n  Type pin # to test..", mode);
      
      while (Serial1.available() == 0 && activeIndex < 0) {
        if (btn.onPress()) {
          activeIndex++;
          activePin = outPins[activeIndex].pin;
          Serial1.printf("\nActive Pin:\n  outPins[%d].pin : %d (%s) \n", activeIndex, outPins[activeIndex].pin, outPins[activeIndex].label);
        }
        btn.poll();
      }
      if (Serial1.available() > 0 && activeIndex < 0) {
        cmd = Serial1.readStringUntil('\n');
        int strToInt = cmd.toInt();
        cmd.toUpperCase();

        if (cmd == "STOP" || cmd == "OFF") break;
        else {
          if (cmd == "BULBS")    activeIndex = 0;
          if (cmd == "SOLENOID") activeIndex = 1;
          if (cmd == "RUMBLE")   activeIndex = 2;
          if (cmd == "CLICK")    activeIndex = 3;
          if (cmd == "LED")      activeIndex = 4;
        }
        if (activeIndex >= 0) activePin = outPins[activeIndex].pin;
        if (!activePin) {
          for (byte i=0; i<=ELEMENTS(outPins); ++i) {
            if (strToInt == outPins[i].pin) {
              activePin   = outPins[i].pin;
              activeIndex = i;
              Serial1.printf("int matched outPins[%d].pin : %d (%s) \n", i, outPins[i].pin, outPins[i].label);
              break;
            }
          } 
          if (!activePin) Serial1.println("\nNO MATCHING OUTPUT PIN FOUND.");
        }
      }
      if (activePin) {
        Serial1.println("\npress Trigger to test pin.");
        if (customDelays) del = customDel[activeIndex];
        while (1) {
          if (btn.onPress()) {
            activeIndex++;
            if (!(activeIndex < ELEMENTS(outPins))) activeIndex = 0;
            if (customDelays) del = customDel[activeIndex];
            activePin = outPins[activeIndex].pin;
            Serial1.printf("\nActive Pin:\n  outPins[%d].pin : %d (%s) \n\npress Trigger to test pin.",activeIndex, outPins[activeIndex].pin, outPins[activeIndex].label);
          }
          if (trigger.onPress()) { 
            digitalWrite(activePin, HIGH), delay(del), digitalWrite(activePin, LOW);
            Serial1.printf("PRESSED: %d (%s) for %d ms.\n", activePin, outPins[activeIndex].label, del);
          }
          if (Serial1.available() > 0) {
            cmd = Serial1.readString(); cmd.toUpperCase();
            if (cmd == "STOP" || cmd == "OFF") break;
            else {
              if (cmd == "BULBS")    activeIndex = 0;
              if (cmd == "SOLENOID") activeIndex = 1;
              if (cmd == "RUMBLE")   activeIndex = 2;
              if (cmd == "CLICK")    activeIndex = 3;
              if (cmd == "LED")      activeIndex = 4;
            }
            if (customDelays) del = customDel[activeIndex];
            activePin = outPins[activeIndex].pin;
          }
          trigger.poll(); btn.poll();
          delay(1);
        } 
      }
      Serial1.println("\nDONE!");
      break;
    case 0:
      Serial1.printf("mode: %d\n", mode);
      break;
    default:
      Serial1.println("default.");
      break;
  }
}
/* END: debugger() */

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

bool gunEnabled() {
  bool en;
  if (gun_en.isPressed()) en = true, setPin(LED, HIGH);
  else en = false, setPin(LED, LOW);
  return en;
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void pollInputs() {
  trigger.poll();
  btn.poll();
  gun_en.poll();
  delay(1);
}

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

void loop() {
  debugMode = true;
  if (debugMode) {
    setPin(LED, HIGH);
    debugger(1), setPin(LED, HIGH);
    //debugger(3,100), setPin(LED, HIGH);
  } else {
    //if (trigger.onPress()) Blink();
    checkTrigger();
    pollInputs();
  }
}
/* END: loop() */
 
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */
/*
void TestFire(int mode, int low = 40, int high = 100) {
  Serial1.printf("\nTestFire:\n  mode: %d\n  low: %d\n  high: %d\n\n",mode,low,high);
  startTime = millis();
  setPin(SOLENOID, HIGH), setPin(BULBS, HIGH), setPin(LED, LOW);
  // 40, 100 = sweet spot?
  if (mode == 2) {
    while ( elapsed() <= (low/2));
    setPin(BULBS, LOW); 
    while ( elapsed() <= low);
    setPin(SOLENOID, LOW); setPin(RUMBLE, HIGH);
    while ( elapsed() <= high);
    setPin(SOLENOID, HIGH);
    while ( elapsed() <= (high+low));
    setPin(SOLENOID, LOW); setPin(RUMBLE, LOW);
    while (elapsed() <= (high*2)); //(recoil*2));
    setPin(LED, HIGH);
  } else if (mode == 1) {
    while (elapsed() < (high*2)+low) {
      if ( elapsed() > (low/2))      setPin(BULBS, LOW); 
      if ( elapsed() > low)          setPin(RUMBLE, HIGH);
      if ( elapsed() > high)         setPin(SOLENOID, LOW); 
      if (elapsed() > (high*2)) {    setPin(RUMBLE, LOW); 
        if (trigger.onPress()) break; //if (trigger.isPressed()) break;
      }
      trigger.poll();
    }
    setPin(SOLENOID, LOW), setPin(RUMBLE, LOW), setPin(BULBS, LOW),setPin(LED, HIGH); 
  }
}
*/
/* END: TestFire() */


/*
States trigState() {
  if (trigger.onPress())            return PRESS;
  else if (trigger.isPressed())     return HELD;
  else if (trigger.onChange() == 2) return REL;
  else                              return REL;
  //else if (trigger.isReleased()) return 0;   
}
*/

/*
 * 
    TOGGLE DOC:
    https://github.com/Dlloydev/Toggle
    
    // onChange:
    if (myInput.onChange() == 2) {
      // button was released
    } else if (myInput.onChange() == 1) {
      // button was pressed
    } else {
      // no change
    }
    /////////////////////////////////////

    // isPressed / isReleased:
    if (myButton.isPressed()) {
      // do something
    } else {
      // do something else
    }
    /////////////////////////////////////

    // pressedFor / releasedFor :
    if (myInput.pressedFor(500)) {
       // true (once only) if button has been pressed for 500ms
    }
    /////////////////////////////////////
    
    if (myInput.retrigger(500)) {
    // count every 500ms interval while the button is being pressed   
    } 
    /////////////////////////////////////

    myInput.getElapsedMs(); // Gets the elapsed ms since the last state change selected by timer mode.
    myInput.clearTimer();   // Simply clears the ms timer used for the timer functions.
    /////////////////////////////////////

    byte pCode = sw1.pressCode(1);
    
    // Up to 225 possible codes with one button. 
    // The returned code (byte) is easy to interpret when viewed in hex format. 
    // -  For example, 47 is 4 long, 7 short presses. 
          F2 is double-click, F7 is 7 Fast clicks.
    // Fast-click mode is detected if the first several clicks (presses) are less than 0.2 sec, 
    // then all presses are counted as fast, up to 15 max (code FF)
    // Detection of long presses occurs if the first press is greater than 0.2 sec, 
    // then all presses greater than 0.2 sec are counted as long 
    // and all presses less than 0.2 sec are counted as short presses.
    /////////////////////////////////////
    
    SETTINGS:
    
    myInput.setSampleUs(us); // Sets the sample period in microseconds. default: 5000 μs.
    
    /////////////////////////////////////
 * 
 */
 
/* ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */