#include <EEPROM.h>
#include <avr/sleep.h> // for sleep mode
#include <avr/power.h> // for sleep mode
#include "config.h"
#include "globals.h"
#include "debug.h"
#include "debounce.h"
//#include "blue_tooth_program"



String message; //string that stores the incoming message


char Incoming_value = 0;

//-------------------------------------------SETUP--------------------------------------------------------
void setup() {
  Serial.begin(9600);
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }




  // -----------------------------------------DEBIOUNCE---------------------------------------------------
  pinMode(TRIGGER_PIN, INPUT);
  pinMode(FIRINGSOLENOID_PIN, OUTPUT);
  pinMode(RECOCKINGSOLENOID_PIN, OUTPUT);
  pinMode (HOPPER_PIN, OUTPUT);

  //-------------------------------------------TLOCK AND PROGRAMMING MODE OPERATION MODE CHECK-------------
  setupPins();
  // Note: do a raw read here , don't use triggerRead() until we're in the main loop
  byte pinState = digitalRead(TLOCK_PIN);
  if ( T_LOCK_ON == pinState ) {
    OperatingMode = MODE_FIRING;
  }

  if ( T_LOCK_OFF == pinState ) {
    byte pinState = digitalRead(TRIGGER_PIN);
    if ( TRIGGER_CLOSED == pinState ) {
      OperatingMode = MODE_PROGRAMMING;
      // at the start of the programming mode, wait for button release if it's still held
      Prog_WaitForTriggerRelease = true;
    }
    else {
      OperatingMode = MODE_FIRING;
    }
  }

  //---------------------------------------------READ REGISTER VALUES---------------------------------------
  //byte programVersion = EEPROM.read(REGISTER_VERSION);
  //if ( programVersion != PROGRAM_VERSION ) {
   // resetProgrammingToDefault();                        // reset programming if program version was updated
  //}

  // read in programming register items
  Conf_Debounce = EEPROM.read(REGISTER_DEBOUNCE);
  Conf_FiringDwell = EEPROM.read(REGISTER_FIRING_DWELL);
  Conf_LoaderDelay = EEPROM.read(REGISTER_LOADER_DELAY);
  Conf_MechDebounce = EEPROM.read(REGISTER_MECH_DEBOUNCE);
  Conf_FSDODwell = EEPROM.read(REGISTER_FSDO_DWELL);
  Conf_FireMode = EEPROM.read(REGISTER_FIRE_MODE);
  Conf_ROFEyesOnInt = EEPROM.read(REGISTER_ROF_ON_INT);
  Conf_ROFEyesOnFrac = EEPROM.read(REGISTER_ROF_ON_FRAC);
  Conf_HopperDwell = EEPROM.read(REGISTER_HOPPER_DWELL);
  Conf_ROFOnOff = EEPROM.read(REGISTER_ROF_ONOFF);
  Conf_DebounceMode = EEPROM.read(REGISTER_DEBOUNCE_MODE);
  Conf_EyesOnOff = EEPROM.read(REGISTER_EYES_ONOFF);
  Conf_DemoMode = EEPROM.read(REGISTER_DEMO_MODE);
  Conf_RecockingDwell = EEPROM.read(REGISTER_RECOCKING_DWELL);
  Conf_ClosedBoltEyeDelay = EEPROM.read(REGISTER_CLOSED_BOLT_EYE_DELAY);
  Conf_BoltClosingDelay = EEPROM.read(REGISTER_BOLT_CLOSING_DELAY);
  Conf_BoardMode = EEPROM.read(REGISTER_BOLT_MODE);
  Conf_RecockingDelay = EEPROM.read(REGISTER_RECOCKING_DELAY);
#ifdef ALLOW_CONFIGURABLE_EYES_OFF_ROF
  Conf_ROFEyesOffInt = EEPROM.read(REGISTER_ROF_OFF_INT);
  Conf_ROFEyesOffFrac = EEPROM.read(REGISTER_ROF_OFF_FRAC);
#endif

  //------------------------------------------SERIAL PRINT FOR BLUETOOTH-------------------------------------------------
 /* Serial.print("Firing Mode (address: 1)(setting: 1-6)="); Serial.println(Conf_FireMode);
  Serial.print("Debounce (address: 2)(setting: 1-10)="); Serial.println(Conf_Debounce);
  Serial.print("Mech Debounce (address: 3)(setting: 1-30)="); Serial.println(Conf_MechDebounce);
  Serial.print("Dwell (address: 4)(setting: 1-30)="); Serial.println(Conf_FiringDwell);
  Serial.print("Loader Delay (address: 5)(setting: 1-10)="); Serial.println(Conf_LoaderDelay);
  Serial.print("FSDO Dwell (address: 6)(setting: 1-5)="); Serial.println(Conf_FSDODwell);
  Serial.print("ROF On/Off (address: 7)(setting: 1-2)="); Serial.println(Conf_ROFOnOff);
  Serial.print("ROF Eyes On Course (address: 8)(setting: 1-27)="); Serial.println(Conf_ROFEyesOnInt);
  Serial.print("ROF Eyes On Fine (address: 9)(setting: 1-10)="); Serial.println(Conf_ROFEyesOnFrac);
  Serial.print("ROF Eyes Off Course (address: 10)(setting: 1-27)="); Serial.println(Conf_ROFEyesOffInt);
  Serial.print("ROF Eyes Off Fine (address: 11)(setting: 1-10)="); Serial.println(Conf_ROFEyesOffFrac);
  Serial.print("Debounce Mode (address: 12)(setting: 1-3)="); Serial.println(Conf_DebounceMode);
  Serial.print("Eyes On/Off (address: 13)(setting: 1-2)="); Serial.println(Conf_EyesOnOff);
  Serial.print("Demo Mode ON/OFF (address: 14)(setting: 1-2)="); Serial.println(Conf_DemoMode);
  Serial.print("Hopper Dwell (address: 15)(setting: 1-30)="); Serial.println(Conf_HopperDwell);
*/

  // default to 10.5 bps EYES OFF
  if ( Conf_ROFOnOff == 2 ) Conf_ROFEyesOffInt = DEFAULT_ROF_EYES_OFF_INT;
  if ( Conf_ROFOnOff == 2 ) Conf_ROFEyesOffFrac = DEFAULT_ROF_EYES_OFF_FRAC;

  //---------------------------------------REGISTER DEFAULTS--------------------------------------------------------------
  if ( Conf_Debounce < 1 || Conf_Debounce > REGISTER_DEBOUNCE_MAX ) Conf_Debounce = DEFAULT_DEBOUNCE;
  if ( Conf_FiringDwell < 1 || Conf_FiringDwell > REGISTER_FIRING_DWELL_MAX ) Conf_FiringDwell = DEFAULT_FIRING_DWELL;
  if ( Conf_LoaderDelay < 1 || Conf_LoaderDelay > REGISTER_LOADER_DELAY_MAX ) Conf_LoaderDelay = DEFAULT_LOADER_DELAY;
  if ( Conf_MechDebounce < 1 || Conf_MechDebounce > REGISTER_MECH_DEBOUNCE_MAX ) Conf_MechDebounce = DEFAULT_MECH_DEBOUNCE;
  if ( Conf_FSDODwell < 1 || Conf_FSDODwell > REGISTER_FSDO_DWELL_MAX ) Conf_FSDODwell = DEFAULT_FSDO_DWELL;
  if ( Conf_FireMode < 1 || Conf_FireMode > REGISTER_FIRE_MODE_MAX ) Conf_FireMode = DEFAULT_FIRE_MODE;
  if ( Conf_ROFEyesOnInt < 1 || Conf_ROFEyesOnInt > REGISTER_ROF_ON_INT_MAX ) Conf_ROFEyesOnInt = DEFAULT_ROF_EYES_ON_INT;
  if ( Conf_ROFEyesOnFrac < 1 || Conf_ROFEyesOnFrac > REGISTER_ROF_ON_FRAC_MAX ) Conf_ROFEyesOnFrac = DEFAULT_ROF_EYES_ON_FRAC;
  if ( Conf_HopperDwell < 1 || Conf_HopperDwell > REGISTER_HOPPER_DWELL_MAX ) Conf_HopperDwell = DEFAULT_HOPPER_DWELL;
  if ( Conf_ROFOnOff < 1 || Conf_ROFOnOff > REGISTER_ROF_ONOFF_MAX ) Conf_ROFOnOff = DEFAULT_ROF_ONOFF;
  if ( Conf_DebounceMode < 1 || Conf_DebounceMode > REGISTER_DEBOUNCE_MODE_MAX ) Conf_DebounceMode = DEFAULT_DEBOUNCE_MODE;
  if ( Conf_EyesOnOff < 1 || Conf_EyesOnOff > REGISTER_EYES_ONOFF_MAX ) Conf_EyesOnOff = DEFAULT_EYES_ONOFF;
  if (Conf_DemoMode < 1 || Conf_DemoMode > REGISTER_DEMO_MODE_MAX) Conf_DemoMode = DEFAULT_DEMO_MODE;
  if (Conf_DemoMode == 2) Conf_EyesOnOff = DEFAULT_EYES_ONOFF;
  if ( Conf_RecockingDwell < 1 || Conf_RecockingDwell > REGISTER_RECOCKING_DWELL_MAX ) Conf_RecockingDwell = DEFAULT_RECOCKING_DWELL;
  if ( Conf_ClosedBoltEyeDelay < 1 || Conf_ClosedBoltEyeDelay > REGISTER_CLOSED_BOLT_EYE_DELAY_MAX ) Conf_ClosedBoltEyeDelay = DEFAULT_CLOSED_BOLT_EYE_DELAY;
  if ( Conf_BoltClosingDelay < 1 || Conf_BoltClosingDelay > REGISTER_BOLT_CLOSING_DELAY_MAX ) Conf_BoltClosingDelay = DEFAULT_BOLT_CLOSING_DELAY;
  if ( Conf_BoardMode < 1 || Conf_BoardMode > REGISTER_BOLT_MODE_MAX ) Conf_BoardMode = DEFAULT_BOLT_MODE;
  if (Conf_RecockingDelay < 1 || Conf_RecockingDelay > REGISTER_RECOCKING_DELAY_MAX) Conf_RecockingDelay = DEFAULT_RECOCKING_DELAY;
  
#ifdef ALLOW_CONFIGURABLE_EYES_OFF_ROF
  if ( Conf_ROFEyesOffInt < 1 || Conf_ROFEyesOffInt > REGISTER_ROF_OFF_INT_MAX ) Conf_ROFEyesOffInt = DEFAULT_ROF_EYES_OFF_INT;
  if ( Conf_ROFEyesOffFrac < 1 || Conf_ROFEyesOffFrac > REGISTER_ROF_OFF_FRAC_MAX ) Conf_ROFEyesOffFrac = DEFAULT_ROF_EYES_OFF_FRAC;
#endif


  // Convert register values to usable program values
  // setting of 1 is 0ms, so subtract one from register value
  Conf_LoaderDelay -= 1;
  Conf_MechDebounce -= 1;
  Conf_FSDODwell -= 1;
  Conf_HopperDwell -= 1;
  //Conf_RecockingDwell -=1;
  //Conf_BoltClosingDelay -=1
  //Conf_ClosedBoltEyeDelay -=1

  // convert ROF register values to numerical BPS value
  Op_ROFEyesOn = convertROFValueNew(Conf_ROFEyesOnInt, Conf_ROFEyesOnFrac);
  Op_ROFEyesOff = convertROFValueNew(Conf_ROFEyesOffInt, Conf_ROFEyesOffFrac);

  // If closed bolt dwell setting is set, change to closed bolt mode
  //if( Conf_RecockingDwell > 1 ) {
  if ( Conf_BoardMode == 2 ) { // FIXME: need constants for this
    Op_BoltMode = BOLTMODE_CLOSED;
  } else {
    Op_BoltMode = BOLTMODE_OPEN;
  }

  //-----------------------------------------------------SET DEBOUNCE-----------------------------------------------------------------
  Debounce_MinTriggerDownCount = Conf_Debounce; // for sequential read debounce
  Debounce_DelayTime = Conf_Debounce; // for delay debounce


  Op_UseROFCap = (Conf_ROFOnOff == 1);  // setup ROF cap


  Trigger_PriorState = Trigger_State = TRIGGER_STATE_WAITING;   // set default trigger state

  //-----------------------------------------------------EYE STARTUP STATUS------------------------------------------------------------
  if (Conf_EyesOnOff == 1) setEyeStatus(EYES_ON);
  if (Conf_EyesOnOff == 2) setEyeStatus(EYES_OFF);



  powerOnLEDBurst(OperatingMode);   // do a little power on color burst like Tadao board does

  if ( MODE_PROGRAMMING == OperatingMode ) {
    Prog_TriggerDownStart = millis();     // track pull time so we can reset the board if held for a certain length of time


    Conf_DebounceMode = DEBOUNCE_MODE_SEQUENTIAL;
    Conf_Debounce = 8;    // programming mode needs to always be locked into a reliable debounce mode
  }

  operationTiming = millis(); // start off generic timer // want this as close to start of main loop as possible
  LastEyeBlinkOn = false;
  LastEyeBlink = operationTiming;
}

int thisByte = 33;

// Main Program Loop
void loop() {
  if ( MODE_FIRING == OperatingMode ) {
    firingMode();

    // blink occasionally to indicate that gun is on
    operatingLEDBlink();
  } else {
    byte pinState = digitalRead(BLUE_TOOTH_ENABLED);

    if (BlueTooth_ON == pinState) {
//      ProgramBlue();

    }
    if (BlueTooth_OFF == pinState) {
      programmingMode();
    }
  }

  //--------------------------------------------DEBOUNCE---------------------------------------------
/*  // read the state of the switch into a local variable:
  int reading = digitalRead(TRIGGER_PIN);

  // check to see if you just pressed the button
  // (i.e. the input went from LOW to HIGH), and you've waited long enough
  // since the last press to ignore any noise:

  // If the switch changed, due to noise or pressing:
  if (reading != lasttriggerState) {
    // reset the debouncing timer
    lastMechDebounceTime = millis();
  }

  if ((millis() - lastMechDebounceTime) > MechdebounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:

    // if the button state has changed:
    if (reading != triggerState) {
      triggerState = reading;

      // only toggle the NOID if the new button state is HIGH
      if (triggerState == HIGH) {
        noidState = !noidState;
      }
    }
  }


  // save the reading. Next time through the loop, it'll be the lasttriggerState:
  lasttriggerState = reading;
  //------------------------------------------------PROGRAM OVER SERIAL PORT------------------------------------------------//
*/


}



/****************************************** Misc. Functions ************************************/
void setupPins() {
  // intialize the tourney lock pin as an input:
  pinMode(TLOCK_PIN, INPUT);
  pinMode(BLUE_TOOTH_ENABLED, INPUT);

  // initialize the solenoid pin(s) as an output:
  //pinMode(FIRINGSOLENOID_PIN, OUTPUT);
  pinMode(HOPPER_PIN, OUTPUT);
  //pinMode(RECOCKINGSOLENOID_PIN, OUTPUT);



  // initialize the pushbutton pin as an input:
  pinMode(TRIGGER_PIN, INPUT);

  // don't need this. using physical pull-up to avoid potential bounce caused by internal pull-up in low voltage situations
  // turn on internal 20kOhm pull-up resistor
  //digitalWrite(TRIGGER_PIN, HIGH);

  // initialize the eye tranceiver pin as an output:
  pinMode(EYE_TX_PIN, OUTPUT);

  // initialize the eye receiver pin as an input:
  pinMode(EYE_RX_PIN, INPUT);

  // Note: RGB LED pins do not need initialization since they're handled with analogWrite()
  // but do this anyway in this function since it is used after we wake from sleep to reset pins
  pinMode(RGBLED_PIN_RED, OUTPUT);
  pinMode(RGBLED_PIN_GREEN, OUTPUT);
  pinMode(RGBLED_PIN_BLUE, OUTPUT);
}


byte getMaxRegisterValue(int regNum) {
  switch ( regNum ) {
    case REGISTER_DEBOUNCE:            return REGISTER_DEBOUNCE_MAX;
    case REGISTER_FIRING_DWELL:               return REGISTER_FIRING_DWELL_MAX;
    case REGISTER_LOADER_DELAY:        return REGISTER_LOADER_DELAY_MAX;
    case REGISTER_MECH_DEBOUNCE:       return REGISTER_MECH_DEBOUNCE_MAX;
    case REGISTER_FSDO_DWELL:          return REGISTER_FSDO_DWELL_MAX;
    case REGISTER_FIRE_MODE:           return REGISTER_FIRE_MODE_MAX;
    case REGISTER_HOPPER_DWELL:        return REGISTER_HOPPER_DWELL_MAX;
    case REGISTER_ROF_ON_INT:          return REGISTER_ROF_ON_INT_MAX;
    case REGISTER_ROF_ON_FRAC:         return REGISTER_ROF_ON_FRAC_MAX;
    case REGISTER_ROF_OFF_INT:         return REGISTER_ROF_OFF_INT_MAX;
    case REGISTER_ROF_OFF_FRAC:        return REGISTER_ROF_OFF_FRAC_MAX;
    case REGISTER_ROF_ONOFF:           return REGISTER_ROF_ONOFF_MAX;
    case REGISTER_DEBOUNCE_MODE:       return REGISTER_DEBOUNCE_MODE_MAX;
    case REGISTER_EYES_ONOFF:          return REGISTER_EYES_ONOFF_MAX;
    case REGISTER_DEMO_MODE:           return REGISTER_DEMO_MODE_MAX;
    case REGISTER_RECOCKING_DWELL:        return REGISTER_RECOCKING_DWELL_MAX;
    case REGISTER_CLOSED_BOLT_EYE_DELAY:    return REGISTER_CLOSED_BOLT_EYE_DELAY_MAX;
    case REGISTER_BOLT_CLOSING_DELAY:   return REGISTER_BOLT_CLOSING_DELAY_MAX;
    case REGISTER_BOLT_MODE:          return REGISTER_BOLT_MODE_MAX;
    case REGISTER_RECOCKING_DELAY:     return REGISTER_RECOCKING_DELAY_MAX;

    default:
      DEBUG_PRINT("ERROR: getMaxRegisterValue(): unrecognized register: ");
      DEBUG_PRINTLN(regNum);
  }
}


//----------------------ROF CODE------------------------------------------------------------//
float convertROFValueNew (float rofInt, float rofFrac) {
  if ( rofInt > 1.00) {
    rofInt -= 2.00;
    rofFrac -= 1.00;
    float actualROF = ((rofInt) + ((rofFrac) * 0.10));
    actualROF += 5.00;
    return actualROF;

  }
  else {
    return ROF_MAX;
  }
}



//-------------END ROF CODE-------------------------------------------------------------------//