/////////////////////////////////////////////
/// Humanitarian Aid Distributor Control ///
///////////////////////////////////////////

/*
A fire control system that can be used with any hardware that "fires" based on the position (and corresponding PWM value) of a single
servo motor.  This system can be used with any number of "positions," with a current limit set to 99;
An array of values is manually user defined in calibration mode.

"triggerPin" pin is attached to the PWM output of a radio receiver and acts as the trigger, bound to a two-position switch.  
"selectorPin" is attached to the PWM output of of a radio receiver, bound to a three-position switch and acts as a fire-selector;
safe / semi / auto.

A calibration script assigns a series of discrete values.  Calibration can be entered in the first n(15) seconds
of power on.  During this window, the button must be held down for n seconds ("long pressed") to enter calibration mode.

The power light blinks slowly in calibration mode.

The user aligns the motor to the desired position using the knob on a rotary encoder, and short presses the button to assign the value.
the LED blinks rapidly several times to indicate the position has been saved.

The user long presses the button again to exit calibration mode, and the LED returns to steady.

The values are saved to EEPROM memory, and loaded again upon reinitalization; the previous calibration is saved throgh power cycles.

// Changes:
  Incorporation of ShiftIn library for handling bitshifters and associated changes

*/

///// LIBRARIES //////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <Servo.h>
#include <EEPROM.h>
#include "ShiftIn.h"  //https://github.com/InfectedBytes/ArduinoShiftIn

///// PINS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////

  
    # define ledPin          4    // LED pin
    # define servoPin        9    // Servo pin
  
  //Fire Control PINS
    # define triggerPin      2    //Used for trigger
    # define selectorPin     3    //Used for fire selector
  
  //ROTARY ENCODER PINS
    //This script was built for and tested on the KY-040 rotary encoder; if using a different encoder check for compatibility and modify script as necessary.

    # define buttonPin       5    // Button pin; note that the button on this and common rotary encoders is PULLUP meaning non-pressed value is HIGH
    # define CLK             6    // CLK pin
    # define DATA            7    // DT pin


  //BITSHIFT PINS
    # define pLoadPin        10   // Connects to Parallel load pin the 165
    # define clockEnablePin  13   // Connects to Clock Enable pin the 165
    # define dataPin         11   // Connects to the Q7 pin the 165
    # define clockPin        12   // Connects to the Clock pin the 165
  
///// VARIABLES //////////////////////////////////////////////////////////////////////////////////////////////////////////////////


  //// GENERAL ////
    # define NUMBER_OF_TUBES            12        // Number of tubes on dropper
    int tubeData[NUMBER_OF_TUBES][2] = {};         //Initiate blank array for tube state and calibration values (CALIBRATION VALUE LOAD NEEDS TO BE STREAMLINED EEPROM)
    
		volatile unsigned long oldArrayPrintClock = millis();						//Clock used for to determine whether or not to print array

    int rearm = 500;                              // Rearm time in ms; represents the minimum time between drops in any mode; on the Peacemaker Mk.0 this is due to mechanical limitations
    volatile bool triggerState = false;             // Variable for if the tirgger is pulled; used this way to allow for ISR with button
    bool selectorSwitch = false;                   // True if using selector switch / safety switch
    unsigned long currentMillis;                  // Variable to store the number of milleseconds since the Arduino has started


  ////BLINKYBOI////
    unsigned long ledMillis = 0;
    int ledState = LOW;

  //// BUTTON LONG / SHORT PRESS ////
    int buttonStatePrevious = HIGH;                     // Previous state of the switch
    unsigned long minButtonLongPressDuration = 3000;    // Time we wait before we see the press as a long press
    unsigned long buttonLongPressMillis;                // Time in ms when we the button was pressed
    bool buttonStateLongPress = false;                  // True if it is a long press
    bool buttonStateShortPress = false;                  // True if short press
    const int intervalButton = 50;                      // Time between two readings of the button state
    unsigned long previousButtonMillis;                 // Timestamp of the latest reading
    unsigned long buttonPressDuration;                  // Time the button is pressed in ms
    int buttonPress = 0;

  ////SHIFT REGISTER////
    #define NUMBER_OF_SHIFT_CHIPS   2               //How many shift register chips are daisy-chained.
    ShiftIn<NUMBER_OF_SHIFT_CHIPS> shifter;         //Creates new shifter instance from ShiftIn.h library
    
    //#define DATA_WIDTH   NUMBER_OF_SHIFT_CHIPS * 8  //Width of data (how many ext lines).
    //#define PULSE_WIDTH_USEC   5                    //Width of pulse to trigger the shift register to read and latch.
    //#define POLL_DELAY_MSEC   1                     //Optional delay between shift register reads.
    //#define BYTES_VAL_T unsigned int                //You will need to change the "int" to "long" If the NUMBER_OF_SHIFT_CHIPS is higher than 2.


///// HARDWARE /////

  /// RADIO CALIBRATION / PWM FIRE CONTROL///
  
    //Fire selector thresholds
      int lowerthresh = 1100;           // This number should be higher than the "low" value on the three position switch, and lower than the "middle" value
      int upperthresh = 1910;           // This number should be higher than the "middle" value on the three position switch, and lower than the "high" value

    //Trigger threshold
      int triggerthresh = 1200;         // This number should be set at the high value of the two-position momentary trigger switch on the radio
      int triggertolerance = 50;        // "Trigger" must be +/- this value

  
  ///SERVO///
    Servo servo;
  
    //SERVO CALIBRATION
      int pwmStep = 5;            // PWM value adjustment for each click of the rotary encoder
      int minPWM = 500;           // Minimum PWM value of servo motor
      int maxPWM = 2500;          // Maximum PWM value of servo motor
      int currentPWM = minPWM;    // (RECONSIDER)Sets the start value to min PWM

    //SERVO VARIABLES
      int pwm_positions[50] = {};// Array of PWM pins to set
      int numberOfTubes = 0;
      int position = 0;



/////////////////////
//// FUNCTIONS: ////
///////////////////



/////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// BUTTON LONG / SHORT PRESS FUNCTION///
void readButtonState() {
  
  //Serial.println("buttons yo"); //test print
  
  currentMillis = millis(); //keep that clock running yo
  buttonStateShortPress = false;  //resets to neutral
  //buttonStateLongPress = false; //not necessary, in fact, probably breaks everything
  
  // If the difference in time between the previous reading is larger than intervalButton
  if(currentMillis - previousButtonMillis > intervalButton) {
    
    // Read the digital value of the button (LOW/HIGH)
    int buttonState = digitalRead(buttonPin);    

    // If the button has been pushed AND
    // If the button wasn't pressed before AND
    // If there was not already a measurement running to determine how long the button has been pressed
    if (buttonState == LOW && buttonStatePrevious == HIGH && !buttonStateLongPress) {
      buttonLongPressMillis = currentMillis;
      buttonStatePrevious = LOW;
      Serial.println("Button pressed");
    }

    // Calculate how long the button has been pressed
    buttonPressDuration = currentMillis - buttonLongPressMillis;

    // If the button is pressed AND
    // If there is no measurement running to determine how long the button is pressed AND
    // If the time the button has been pressed is larger or equal to the time needed for a long press
    if (buttonState == LOW && !buttonStateLongPress && buttonPressDuration >= minButtonLongPressDuration) {
      buttonStateLongPress = true;
      Serial.println("Button long pressed");
    }
      
    // If the button is released AND
    // If the button was pressed before
    if (buttonState == HIGH && buttonStatePrevious == LOW) {
      buttonStatePrevious = HIGH;
      buttonStateLongPress = false;
      Serial.println("Button released");

      // If there is no measurement running to determine how long the button was pressed AND
      // If the time the button has been pressed is smaller than the minimal time needed for a long press
      if (buttonPressDuration < minButtonLongPressDuration) {
        buttonStateShortPress = true;
        Serial.println("Button pressed shortly");
      }
    }
    
    // store the current timestamp in previousButtonMillis
    previousButtonMillis = currentMillis;
  }

}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////




/////////////////////////////////////////////////////////////////////////////////////////////////////////////

///EEPROM ///

/// WRITE INT-ARRAY EEPROM FUNCTION ///
  // Type dependent; "2" is for INT, "4" for long etc...
  // Adapted from https://roboticsbackend.com/arduino-store-array-into-eeprom/

void writeIntArrayIntoEEPROM(int address, int numbers[], int arraySize)
{
  int addressIndex = address;
  for (int i = 0; i < arraySize; i++) 
  {
    EEPROM.write(addressIndex, numbers[i] >> 8);
    EEPROM.write(addressIndex + 1, numbers[i] & 0xFF);
    addressIndex += 2;
  }
}



///READ INT-ARRAY EEPROM FUNCTION ///
  //Type dependent; "2" is for INT, "4" for long etc...
  //Adapted from https://roboticsbackend.com/arduino-store-array-into-eeprom/
  
void readIntArrayFromEEPROM(int address, int numbers[], int arraySize)
{
  int addressIndex = address;
  for (int i = 0; i < arraySize; i++)
  {
    numbers[i] = (EEPROM.read(addressIndex) << 8) + EEPROM.read(addressIndex + 1);
    addressIndex += 2;
  }
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////






/////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// BLINK FUNCTION ///

  //Adapted from https://www.arduino.cc/en/Tutorial/BuiltInExamples/BlinkWithoutDelay
  
void blinkyBoi(long interval)
{  
  currentMillis = millis();    // Store the current time
  
  if (currentMillis - ledMillis >= interval) {
   
    ledMillis = currentMillis; // Save the last time you blinked the LED

    // If the LED is off turn it on and vice-versa:
    if (ledState == LOW) {
      ledState = HIGH;
    } else {
      ledState = LOW;
    }

    
    digitalWrite(ledPin, ledState); // Set the LED with the ledState of the variable:
  }
  
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////




/////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// READ ROTARY ///

  //This is adapted from https://www.best-microcontroller-projects.com/rotary-encoder.html.
  //It is a no-capacitor solution for accurately reading cheap encoders, such as the KY-040 used here.
  
static uint8_t prevNextCode = 0;
static uint16_t store=0;

  // A valid CW or  CCW move returns +/- 1, invalid returns 0.
int8_t read_rotary() {
  static int8_t rot_enc_table[] = {0,1,1,0,1,0,0,1,1,0,0,1,0,1,1,0};

  prevNextCode <<= 2;
  if (digitalRead(DATA)) prevNextCode |= 0x02;
  if (digitalRead(CLK)) prevNextCode |= 0x01;
  prevNextCode &= 0x0f;

   // If valid then store as 16 bit data.
   if  (rot_enc_table[prevNextCode] ) {
      store <<= 4;
      store |= prevNextCode;
      //if (store==0xd42b) return 1;
      //if (store==0xe817) return -1;
      if ((store&0xff)==0x2b) return -1;
      if ((store&0xff)==0x17) return 1;
   }
   return 0;
}

/////////////////////////////////////////////////////////////////////////////////////////////////////////////




///SHIFT REGISTERS//////////////////////////////////////////////////////////////////////////////////////////////////////////


void retrieveShiftValues(){
  if(shifter.update()){
		for(int i = 0; i < NUMBER_OF_TUBES; i++)
    	tubeData[(i)][0] = shifter.state(shifter.getDataWidth()-1-i); // get state of button i and add to array
	}
}

void displayValues() {
  for(int i = 0; i < shifter.getDataWidth(); i++)
    Serial.print( shifter.state(i) ); // get state of button i
  Serial.println();
}





/// READ SHIFT REGISTERS///
  //This function loads the values of shift registers into the defined target
/*
void read_shift_regs()
{
    long bitVal;
    BYTES_VAL_T bytesVal = 0;

    //Trigger a parallel Load to latch the state of the data lines,
    
    digitalWrite(clockEnablePin, HIGH);
    digitalWrite(ploadPin, LOW);
    delayMicroseconds(PULSE_WIDTH_USEC);
    digitalWrite(ploadPin, HIGH);
    digitalWrite(clockEnablePin, LOW);

    // Loop to read each bit value from the serial out line of the SN74HC165N.
 
    for(int i = 0; i < DATA_WIDTH; i++)
    {
        bitVal = digitalRead(dataPin);

        // Set the corresponding bit in bytesVal.
       
        bytesVal |= (bitVal << ((DATA_WIDTH-1) - i));

        // Pulse the Clock (rising edge shifts the next bit).
        
        digitalWrite(clockPin, HIGH);
        delayMicroseconds(PULSE_WIDTH_USEC);
        digitalWrite(clockPin, LOW);
    }

    return(bytesVal);
}*/

/*
///DISPLAY STORED BITREGISTER VALUES FROM INT///

void displayValues() {
  // print out all buttons
  for(int i = 0; i < shifter.getDataWidth(); i++)
    Serial.print( shifter.state(i) ); // get state of button i
  Serial.println();
}*/

/////////////////////////////////////////////////////////////////////////////////////////////////////////////


///ARRAY FUNCTIONS///////////////////////////////////////////////////////////////////////////////////////////////////////


///ARRAY BULLSHIT THAT NEEDS FIXING ///
		//This just steals the array from the old dArray calibrate function.  Maybe that's fine.
void move_array(){
  for (int i = 1; i < (NUMBER_OF_TUBES +1); i++) {
  int dArray = i;
  int ddArray = i-1;
  tubeData[ddArray][1] = pwm_positions[dArray];
  
  }
}


///ddArray Print Function///
void print_dd_array(int matrix[NUMBER_OF_TUBES][2]){ 
	
	if(millis() - oldArrayPrintClock >= 5000){
  	for(int a = 0; a < NUMBER_OF_TUBES; a++)
  	{
		Serial.print("Tube "); Serial.print(a); Serial.print(": ");
    for(int b = 0; b < 2; b++)
    	{	
      Serial.print(matrix[a][b]);
      Serial.print(" ");
    	}
		Serial.println();
  	}
		oldArrayPrintClock = millis(); 
	}
}



/////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// CALIBRATE SERVO FUNCTION///

void calibrate()
{

static uint8_t prevNextCode = 0;
static uint16_t store=0;

bool escapeCalibrate = false;
unsigned long calibrateTimeInit = millis();  //Get calibrate initialized time stamp
unsigned long calibrateTimeCurrent = 0;
unsigned long calibrateTimeElapsed = 0;
  
numberOfTubes = 0; //this will be stored in the first value of the array and will be used to instruct the dropping function of the number of tubes / positions
  
  //Serial.println("Beginning calibration.");

  
  while(escapeCalibrate == false){
    static int c,val;
    
    //Serial.println("calibrating");//testprint
    blinkyBoi(300); //blinks LED at specified rate in MS while in calibration mode

    
    
   
    //start local clock for exit criteria;
      calibrateTimeCurrent = millis();
      calibrateTimeElapsed = calibrateTimeCurrent - calibrateTimeInit;
    

  //Get rotary position
   if( val=read_rotary() ) {
      c +=val;
      
      //Confine rotary position to useful values based on defined PWM range for servo
        if(c < 0){
          c = 0;
        }else if(c > (ceil((maxPWM - minPWM)/pwmStep))){
          c = ceil((maxPWM - minPWM)/pwmStep);
          }

    //Convert position to PWM

    currentPWM = minPWM + c * pwmStep;

          Serial.println(currentPWM);
          servo.writeMicroseconds(currentPWM);
   }
  
  //SAVE CURRENT PWM VALUE TO ARRAY/VECTOR
    if(buttonStateShortPress == true){
      
      //Increase numberOfTubes by one and write to array/vector 0
      numberOfTubes++;
      pwm_positions[0]=numberOfTubes;
      
      //write PWM value to array/vector
      pwm_positions[numberOfTubes] = currentPWM;
      
      //Rapidly blink to acknowledge input
     
        for (int j = 1; j <= 5; ++j)
        {
        // if the LED is off turn it on and vice-versa:
        if (ledState == LOW) {
            ledState = HIGH;
            digitalWrite(ledPin, ledState);
            delay(50); // rapid blink delay
        } else 
            {
          ledState = LOW;
         digitalWrite(ledPin, ledState);
         delay(50); // rapid blink delay
         
        }
          
      }
    }
  
    
    /*
  //Serial Print Array (test)
    
    for(int i = 0; i < (numberOfTubes+1); i++)
  {
    Serial.println(pwm_positions[i]);
  }*/
    
       
  
  //escape?
  readButtonState();
 
    //Exit not allowed for n seconds
    if(buttonStateLongPress == true && (calibrateTimeElapsed > 10000))
    {
    escapeCalibrate = true;
    buttonStateLongPress = false; //resets the long press upon exit
    //Serial.println(escapeCalibrate);
      
    //UPDATE ARRAY TO EEPROM 
    writeIntArrayIntoEEPROM(10,pwm_positions,(numberOfTubes+1));

    //UPDATE PRIMARY ARRAY
    move_array();

    //TURN LED ON
    digitalWrite(ledPin, HIGH);

    servo.writeMicroseconds(pwm_positions[1]); // set servo to beginning of sequence
    } 
  } 
}
  
/////////////////////////////////////////////////////////////////////////////////////////////////////////////





//////Fire Control Functions//////////////////////////////////////////////////////////////////////////////
  

///Trigger Function///
  //Button Trigger ISR
  void trigger_ISR(){
         // read the state of the pushbutton value:
          int pinState = digitalRead(triggerPin);

          switch (pinState){
            case 0: 
              triggerState = false;
            case 1: 
              triggerState = true;
          }
}

  //Button Trigger
    bool trigger_pulled(){
       // read the state of the pushbutton value:
          int pinState = digitalRead(triggerPin);

          switch (pinState){
            case 0: 
              triggerState = false;
              return false;
            case 1: 
              triggerState = true;
              return true;
          }
    }



  /* // PWM BASED TRIGGER
    bool trigger_pulled(){
       int trigger_value = pulseIn(triggerPin, HIGH); //reads the value of the trigger
       
      //Checks the trigger value against acceptable trigger range
       if (((trigger_value > (triggerthresh - triggertolerance)) && (trigger_value < (triggerthresh + triggertolerance)))){
        triggerState = true;
        return true;
       } else
          triggerState = false;
          return false;
    }*/





///Safety / Fire Selector Function///
  //This function is for a PWM-based signal for the fire selector.  If there is no fire selector as defined in the parameters
  //this function will always return "semiautomatic" mode.
              
int fire_selector(){
  
  if (selectorSwitch == false) return 1; //check to see if there's a selector switch, if no, return mode 1

  //PWM receiver based selector
  
    //Read 3 position switch on radio controller to use as fire selector; safe / semi / full auto
    //get PWM value from fire seloctor 3 position switch on radio
    int selector_value = pulseIn(selectorPin, HIGH);


    //semi-auto mode "1"
    if ((lowerthresh < selector_value) && (selector_value < upperthresh)){
      return 1; 
    } else 

    //Full-auto mode "2"
    if (selector_value > upperthresh){
    return 2;       
    } else
    
    //Any other, "safe" mode "0"     
    return 0; 
    
}



///Fire Dropper///

void fire_dropper(){

    // set the servo to the next position in the array
        position++; //Increment through assigned values in array
        servo.writeMicroseconds(pwm_positions[position]);  //Move servo
        delay(rearm);
        

    
    //Resets servo to default position upon expenditure / loading of all rounds
      if (position == numberOfTubes+1){
            
            position = 1;
            servo.writeMicroseconds(pwm_positions[position]); // begins reset
            delay(2000); // allows time for servo to move to reset position
        }
}




/////////////////////////////////////////////////////////////////////////////////////////////////////////////





/////////////////////////////////////////////////////////////////////////////////////////////////////////////

///////////////////////
/// SETUP FUNCTION ///
/////////////////////

void setup() 
{
  
  //SERIAL SETUP
  
    Serial.begin(9600);                 // Initialise the serial monitor


  //BUTTON PINS
    //pinMode(buttonPin, INPUT);          // set buttonPin as input for standard physical button
    pinMode(buttonPin, INPUT_PULLUP);     //Button on rotary encoder using INPUT_PULLUP

  //Fire Control Setup
    //Button Fire Control
      pinMode(triggerPin, INPUT); //used for trigger
      attachInterrupt(digitalPinToInterrupt(triggerPin), trigger_ISR, RISING);


    
    //Radio Setup // PWM Fire Control
      //pinMode(triggerPin, INPUT); //used for trigger
      //pinMode(selectorPin, INPUT); // used for fire selector
  
  //SERVO SETUP
    servo.attach(servoPin);                                 //assigns pin 9 to servo output
    servo.writeMicroseconds(pwm_positions[(position+1)]);   //Default the servo to first alignment value upon initialization
    readIntArrayFromEEPROM(10,pwm_positions,50);           //Read array from EEPROM
    move_array();                                         //Write these values to the bigger array (!!!!!)
    
    numberOfTubes = pwm_positions[0];                       //initializes variable from memory


  //BITSHIFT SETUP
      shifter.begin(pLoadPin, clockEnablePin, dataPin, clockPin);  // Assigns pins to the shifter
 
  
  //Main LED
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH); //defaults the light "on" for power

  
}
/////////////////////////////////////////////////////////////////////////////////////////////////////////////










/////////////////////////////////////////////////////////////////////////////////////////////////////////////

//////////////////////
/// MAIN FUNCTION ///
////////////////////
void loop(){
  //Serial.println("Main Loop");// test print
  currentMillis = millis();    // store the current time
  
	int oldtubeData[NUMBER_OF_TUBES][2] = {};  
  
  //only check for button press to trigger calibration during the first 15 seconds after initialization
  if(currentMillis < 10000){
  readButtonState(); // read the button state
   if (buttonStateLongPress == true)
   { 
   //Serial.println("I am here");
   calibrate();
   }
  }


//// FIRE CONTROL ////
//Serial.println(triggerState);


  //Is trigger pulled?
  if(triggerState){
    int selectorMode = fire_selector();

    switch (selectorMode){
      case 0: //SAFE
          //don't drop grenades lmao
          //maybe a way to prevent any servo movement whatsoever if this is enabled?
        
        break;
        
      case 1: //SEMI
              // doesn't currently allow rapid fire; if trigger is pulled faster than rearm time the second trigger pull won't fire when rearm is complete
              //could possibly solve by implementing a clock logic to count time, that could then be used in boolean logic?
       
        fire_dropper();
        
        while (trigger_pulled()){
          //Serial.println("you're here");
          } //Waits for the trigger to reset before proceeding 
        break;

      case 2: //AUTO
        fire_dropper();
        trigger_pulled();
        break;
    }  
  }
		//displayValues();
		retrieveShiftValues();
  	print_dd_array(tubeData); // Prints the main array

  delay(100); // just for simulation
}//Good hunting.