#include <SoftwareSerial.h>   // #ifdef Arduino
#include <Adafruit_NeoPixel.h>

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++     PIN  DECLARATIONS  ++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// Potentiometer that will determine how difficult things are
int Difficulty_sensor_pin = A0;

// Here are the different pins or the different sensors behind the four tiles
const int TL_tile_sensor_pin = A5;
const int BL_tile_sensor_pin = A4;
const int TR_tile_sensor_pin = A3;
const int BR_tile_sensor_pin = A2;

// Here are the pin definitions for the different LEDs
const int L_force_LED_pin = 8;
const int R_force_LED_pin = 9;
const int Target_LED_pin  = 10;

// The following are for the software serial port.  They are only really necessary if am using ATMEGA328 (rather than Arduino)
const int rx_pin = 0; // this maps to the TX on the FDDI board
const int tx_pin = 1; // this maps to the RX on the FDDI board
SoftwareSerial ATMegaSerial(rx_pin, tx_pin);                     // This is only needed if going stright from ATMEGA

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++     PROGRAM  DECLARATIONS  ++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// The following variables are just to help with readability
// The following are codes I'll use to determine colors and states of the different target tiles
const int try_to_hit_code = 1;
const int hit_code = 2;
const int miss_code = 3;

// This variable will allow us to set how big a handicap can be.
// The range amount will be added when the potentiometer is at the 
// maximum position.  Depending on how sensitive the sensors are, this number
// can be increased/decreased.  Essencially, this sets the "sensitivity" and impact
// of the difficulty potentiometer.  If range is set to low number, then
// changing the potentiometer will have little impact.  If made as high 
// as the number of LEDs in the force sensors, then a small hit will 
// light up all LEDs. 
const int handicap_range = 0;    // Setting to 0 makes it so handicap is ignored
int current_handicap = 0;
int sensor_max_range = 225;      
int minimum_force_to_trigger = 2;  // This is on a scale from 2 to the number of force LEDs

// This will set how much debugging we want
const int debugging_level = 3;        // Sets amount of debugging will output: 1=terse, 4=extremely verbose 
const int no_value_constant = -99;    // This is just used for when don't have value to output in debugging
const int sensors_to_track = 11;       // This limits which sensors to track for debugging as need to do one at a time
                                      // Am using bitwise & to determine if sensor should be set.
                                      //  1st bit (1s place) is for bottom right sensor
                                      //  2nd bit (2s place) is for bottom left sensor
                                      //  3rd bit (4s place) is for top left sensor
                                      //  4th bit (8s place) is for top right sensor
                                      //  Example: 5 would mean bits 1 and 3 are set, so BR and TL would be monitored
                                      //  Example: 15 would mean all tiles

// The following tile constants will be used
// The are ordered in the order that they will be along the LED strip
int BR_tile = 1;
int BL_tile = 2;
int TL_tile = 3;
int TR_tile = 4;
int number_of_tiles = 4;

// The following will represent the steady state for each tile's sensor
// It is this value we'll use to determine if the tile was hit
int TL_baseline = 0;
int TR_baseline = 0;
int BL_baseline = 0;
int BR_baseline = 0;

// The following variable is going to be used to determine to store the difficulty level.

// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// +++++++++++++++++++++     LED  DECLARATIONS  ++++++++++++++++++++++++
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// The following declarations are for all the LED effects and variables.
// Sets the number of LEDs in the different strips
int num_of_LEDs_in_force_strips = 9;
int num_of_LEDs_in_target_strip = 4;

// Parameter 1 = number of pixels in strip
// Parameter 2 = pin number (most are valid)
// Parameter 3 = pixel type flags, add together as needed:
//   NEO_RGB     Pixels are wired for RGB bitstream
//   NEO_GRB     Pixels are wired for GRB bitstream
//   NEO_KHZ800  800 KHz bitstream (e.g. High Density LED strip)
Adafruit_NeoPixel L_force_strip = Adafruit_NeoPixel(num_of_LEDs_in_force_strips, L_force_LED_pin, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel R_force_strip = Adafruit_NeoPixel(num_of_LEDs_in_force_strips, R_force_LED_pin, NEO_GRB + NEO_KHZ800);
Adafruit_NeoPixel Target_strip  = Adafruit_NeoPixel(num_of_LEDs_in_target_strip, Target_LED_pin, NEO_GRB + NEO_KHZ800);

// Sets the visual effect on the force LED strips
int per_LED_lighting_delay = 50;  // 25 milliseconds.  This is what determines how quickly the lights light up as the strip gets displayed
int per_lighting_delay = 250;  // 1000 milliseconds.  This determines how long the FORCE LEDs stay lit up

// +++++++++++++++++++++++++++++++++++       setup()       +++++++++++++++++++++++++++++++
void setup() {
  //SoftwareSerial ATMegaSerial(rxPin, txPin); // RX, TX
  ATMegaSerial.begin(9600);  
  debug(1, "++++++++++++  Am doing setup()", no_value_constant);
                                   
  // This line enables the input of the AREF pin.  This pin will be used to lower the voltage reference on the sensors
  analogReference(INTERNAL);
  debug(2, "++  Setting analogReference to External", no_value_constant);


  // The following initialize the different LED strips
  L_force_strip.begin();    // turns strip on
  L_force_strip.show();     // initializes pixels to off
  R_force_strip.begin();    // turns strip on 
  R_force_strip.show();     // initializes pixels to off
  Target_strip.begin();     // turns strip on
  Target_strip.show();      // initializes pixels to off

  // The following set the pins that turn the lights on so that they output the values they need to
  pinMode(L_force_LED_pin, OUTPUT);
  pinMode(R_force_LED_pin, OUTPUT);
  pinMode(Target_LED_pin,  OUTPUT);
  
  /*
  pinMode(TL_tile_sensor_pin, INPUT_PULLUP);
  pinMode(TR_tile_sensor_pin, INPUT_PULLUP);
  pinMode(BL_tile_sensor_pin, INPUT_PULLUP);
  pinMode(BR_tile_sensor_pin, INPUT_PULLUP);
  */
  
  // This is just so can make sure target LEDs are wired correctly
  // run_target_LED_test();
  
  // Calibrate the sensors by finding the steady state values
  calibrate_sensors();

} // setup()

// +++++++++++++++++++++++++++++++++++       loop()       +++++++++++++++++++++++++++++++
void loop() {
  debug(1, "++++++++++++  Am doing loop()", no_value_constant);

  get_difficulty_level();                              // Get potentiometer value for difficulty.
  // This will determine how sensitive the pads should be
  // Need to check this every time loop because it could change in between runnings

  int target_to_hit = random(1, number_of_tiles + 1);  // (Randomly) Pick a target to hit
  light_target_strip(target_to_hit, try_to_hit_code);  // Now light the target tile

    wait_for_hit(target_to_hit);                         // Loop until one of the tiles gets hit

} // loop()

// ++++++++++++++++++++++++++++++++       wait_for_hit()       +++++++++++++++++++++++++++++
// This routine will loop until one of the tiles is hit.  Once it's hit, it will return the tile that was hit.
void wait_for_hit(int tile_need_to_hit) {
  debug(1, "++++++++++++  Am doing wait_for_hit()- Target tile", tile_need_to_hit);
  int tile_hit = 0;
  int force_hit_with = 0;

  debug(2, "Am waiting for a hit", no_value_constant);
  while (tile_hit == 0) {
    delay(100);

    if ((sensors_to_track & B00000100) == B00000100) {
      int TL_sensor_reading = analogRead(TL_tile_sensor_pin);
      int TL_force_level = map(TL_sensor_reading, TL_baseline, TL_baseline + sensor_max_range, 1, num_of_LEDs_in_force_strips);
      
      debug(4, "++ TL_sensor_reading", TL_sensor_reading);        
      
      if (TL_force_level >= minimum_force_to_trigger) {
        tile_hit = TL_tile;
        force_hit_with = TL_force_level;
        debug(2, "++ TL TILE HIT ", TL_tile);
        debug(3, "++ TL baseline ", TL_baseline);
        debug(3, "++ TL reading  ", TL_sensor_reading);
        debug(3, "++ TL max value", TL_baseline + sensor_max_range);   
      } // (TL_force_level > 1)
    } // (sensors_to_track = 4)

  if ((sensors_to_track & B00001000) == B00001000) {
      int TR_sensor_reading = analogRead(TR_tile_sensor_pin);     
      int TR_force_level = map(TR_sensor_reading, TR_baseline, TR_baseline + sensor_max_range, 1, num_of_LEDs_in_force_strips);
      
      debug(4, "++ TR_sensor_reading", TR_sensor_reading);        
      
      if (TR_force_level >= minimum_force_to_trigger) {
        tile_hit = TR_tile;
        force_hit_with = TR_force_level;
        debug(2, "++ TR TILE HIT ", TR_tile);
        debug(3, "++ TR baseline ", TR_baseline);
        debug(3, "++ TR reading  ", TR_sensor_reading);
        debug(3, "++ TR max value", TR_baseline + sensor_max_range);
      } //(TR_force_level > 1)
   } // (sensors_to_track == 8)
   
  if ((sensors_to_track  & B00000010) == B00000010) {
      int BL_sensor_reading = analogRead(BL_tile_sensor_pin); 
      int BL_force_level = map(BL_sensor_reading, BL_baseline, BL_baseline + sensor_max_range, 1, num_of_LEDs_in_force_strips);
      
      debug(4, "++ BL_sensor_reading", BL_sensor_reading);      
      
      if (BL_force_level >= minimum_force_to_trigger) {
        tile_hit = BL_tile;
        force_hit_with = BL_force_level;
        debug(2, "++ BL TILE HIT ", BL_tile);
        debug(3, "++ BL baseline ", BL_baseline);
        debug(3, "++ BL reading  ", BL_sensor_reading);
        debug(3, "++ BL max value", BL_baseline + sensor_max_range);
        debug(1, "Got here and shouldn't", no_value_constant);
      } // (BL_force_level > 1)
   } // (sensors_to_track == 2)
   
  if ((sensors_to_track & B00000001) == B00000001) {
      int BR_sensor_reading = analogRead(BR_tile_sensor_pin); 
      int BR_force_level = map(BR_sensor_reading, BR_baseline, BR_baseline + sensor_max_range, 1, num_of_LEDs_in_force_strips);

      debug(4, "++ BR_sensor_reading", BR_sensor_reading);        
      
      if (BR_force_level >= minimum_force_to_trigger) {
        tile_hit = BR_tile;
        force_hit_with = BR_force_level;
        debug(2, "++ BR TILE HIT ", BR_tile); 
        debug(3, "++ BR baseline ", BR_baseline);
        debug(3, "++ BR reading  ", BR_sensor_reading);
        debug(3, "++ BR max value", BR_baseline + sensor_max_range);
      } // (BR_force_level > 1)
   } // (sensors_to_track == 1)
   
  } // while (tile_hit == 0)

  if (tile_hit == tile_need_to_hit) {
    light_target_strip(tile_hit, hit_code);  // This means we hit the tile they were supposed to
  }
  else { // (tile_hit != tile_need_to_hit)
    light_target_strip(tile_hit, miss_code);  // This means they missed the target tile
  }

  int adjusted_force_measurement = force_hit_with + current_handicap;   // This adjusts the force level and it takes the difficulty handicap into account
  
  // Need to make sure the difficulty adjustment doesn't put into negative territory...
  if (adjusted_force_measurement < 1) {
    adjusted_force_measurement = 1;
  }
  
  if (adjusted_force_measurement > num_of_LEDs_in_force_strips) {
    adjusted_force_measurement = num_of_LEDs_in_force_strips;
  }
  
  debug(3, "++ Force Level", force_hit_with);   
  debug(3, "++ Adjusted Level", adjusted_force_measurement);
  
  // Need to fix in case have negative value with the handicap
  light_force_strips(adjusted_force_measurement);
 
  L_force_strip.show();     // initializes pixels to off
  R_force_strip.show();     // initializes pixels to off 
  
  //delay(2000);
} // wait_for_hit


// ++++++++++++++++++++++++++++++++       get_difficulty_level()       +++++++++++++++++++++++++++++
// Each time we loop through, we need to check the potentiometer to determine how hard to make all this.  
//
// 
void get_difficulty_level() {
  debug(1, "++++++++++++  Am doing get_difficulty_level()", no_value_constant);

  int  Difficulty_setting = analogRead(Difficulty_sensor_pin);
  debug(4, "Difficulty Reading", Difficulty_setting);   

  current_handicap = map(Difficulty_setting, 1, 1023, -handicap_range, handicap_range);
  debug(4, "Difficulty Handicap", current_handicap);

} // get_difficulty_level() 



// ++++++++++++++++++++++++++++++++       calibrate_sensors()       +++++++++++++++++++++++++++++
// This will be called once at the startup of the program.  It will do the following...
// Read through each sensor 10 times and get the average value over five seconds.  
// That average will be used in the rest of the program as the baseline/steady state
// when nothing is happening.
//
// And to make it more fun (and to provide visual feedback, I will light the side/force
// LEDs 
void calibrate_sensors() {
  debug(1, "++++++++++++  Am doing calibrate_sensors()", no_value_constant);

  int TL_total_readings = 0;
  int TR_total_readings = 0;
  int BL_total_readings = 0;
  int BR_total_readings = 0;

  debug(3, "TL initial read: ", analogRead(TL_tile_sensor_pin));
  debug(3, "TR initial read: ", analogRead(TR_tile_sensor_pin));
  debug(3, "BL initial read: ", analogRead(BL_tile_sensor_pin));
  debug(3, "BR initial read: ", analogRead(BR_tile_sensor_pin));  

  for(int reading=1; reading < num_of_LEDs_in_force_strips; reading++) {
    TL_total_readings = TL_total_readings + analogRead(TL_tile_sensor_pin);
    TR_total_readings = TR_total_readings + analogRead(TR_tile_sensor_pin);
    BL_total_readings = BL_total_readings + analogRead(BL_tile_sensor_pin);
    BR_total_readings = BR_total_readings + analogRead(BR_tile_sensor_pin);

    // This will light the strips so that people can visually see how much longer have before is calibrated
    // It will also ensure there's a slight pause
    light_force_strips(reading);

    debug(4, "++ Doing reading: ", reading);   
    debug(4, "TL total: ", TL_total_readings);
    debug(4, "TR total: ", TR_total_readings);
    debug(4, "BL total: ", BL_total_readings);
    debug(4, "BR total: ", BR_total_readings);  

  } //for(int readings; readings < 10; readings++)

  TL_baseline = int(TL_total_readings / num_of_LEDs_in_force_strips);
  TR_baseline = int(TR_total_readings / num_of_LEDs_in_force_strips);
  BL_baseline = int(BL_total_readings / num_of_LEDs_in_force_strips);
  BR_baseline = int(BR_total_readings / num_of_LEDs_in_force_strips);

  debug(1, "TL calibrated baseline: ", TL_baseline);
  debug(1, "TR calibrated baseline: ", TR_baseline);
  debug(1, "BL calibrated baseline: ", BL_baseline);
  debug(1, "BR calibrated baseline: ", BR_baseline);

  if  ((sensors_to_track & B00000001) == B00000001) {
      debug(2, "++ Am tracking BR sensor", no_value_constant);
  }
  if  ((sensors_to_track & B00000010) == B00000010) {
      debug(2, "++ Am tracking BL sensor", no_value_constant);
  }
  if  ((sensors_to_track & B00000100) == B00000100) {
      debug(2, "++ Am tracking TL sensor", no_value_constant);
  }
  if  ((sensors_to_track & B00001000) == B00001000) {
      debug(2, "++ Am tracking TR sensor", no_value_constant);
  }
  
} // calibrate_sensors() 


// ++++++++++++++++++++++++++++++++       light_target_strip()       +++++++++++++++++++++++++++++
// This will be used to light the FORCE_LEDs
// Fill the dots one after the other with a color
void light_target_strip(int tile_to_light, int color_to_set_to) {
  debug(1, "++++++++++++  Am light_target_strip()- Tile lighting", tile_to_light);
  debug(4, "Setting color to", color_to_set_to);

  tile_to_light = tile_to_light - 1;    // Need to decrement because NeoPixels start at 0
  
  uint32_t color_to_set = 0;

  if (color_to_set_to == try_to_hit_code) {
    color_to_set = Target_strip.Color(0,0,255); // blue
  }
  else if (color_to_set_to == hit_code) {
    color_to_set = Target_strip.Color(0,255,0); // green
  }
  else if (color_to_set_to == miss_code) {
    color_to_set = Target_strip.Color(255,0,0); // red
  }

  for(int tile = 0; tile < number_of_tiles; tile++) {
    if(tile_to_light == tile) { // set to color green
      Target_strip.setPixelColor(tile, color_to_set);
    }  // if
    else {  // set the rest to blank if not the tile_to_light
      Target_strip.setPixelColor(tile, Target_strip.Color(0,0,0));
    }  // else
  } // for(int tile; tile < number_of_tiles)
  
  Target_strip.show();
} // light_target_strip



// +++++++++++++++++++++++++++++++       light_force_strips()       ++++++++++++++++++++++++++++
// This will be used to light the FORCE_LEDs
// Fill the dots one after the other with a color
void light_force_strips(int pixelsToLight) {
  debug(1, "++++++++++++  Am doing light_force_strips()- PixelsToLight", pixelsToLight);

  // The following will determine the color to use when lighting the force LEDs.  
  int red_to_use = map(pixelsToLight, 0, num_of_LEDs_in_force_strips, 255, 1);
  int green_to_use = map(pixelsToLight, 0, num_of_LEDs_in_force_strips, 1, 255);
  int blue_to_use = 0;

  // light them up
  for(int i=0; i<=pixelsToLight; i++) {
    L_force_strip.setPixelColor(i, L_force_strip.Color(red_to_use, green_to_use, blue_to_use));
    R_force_strip.setPixelColor(i, R_force_strip.Color(red_to_use, green_to_use, blue_to_use));
    L_force_strip.show();
    R_force_strip.show();
    delay(per_LED_lighting_delay);
  } // for

  delay(per_lighting_delay);

  // turn them off
  for(int i = pixelsToLight; i>=0; i--) {
    L_force_strip.setPixelColor(i, L_force_strip.Color(0, 0, 0));
    R_force_strip.setPixelColor(i, R_force_strip.Color(0, 0, 0));
    L_force_strip.show();
    R_force_strip.show();
    delay(per_LED_lighting_delay);
  } // for
} //lightStrip


// +++++++++++++++++++++++++++++++       debug(level, msg)       ++++++++++++++++++++++++++++
void debug(int level, String msg, int value) {
  if (level <= debugging_level) {
    if (value > no_value_constant) {
      ATMegaSerial.print(msg);
      ATMegaSerial.print(":   ");
      ATMegaSerial.println(value);
    } // (value > no_value_constant)
    else {
      ATMegaSerial.println(msg);
    }
  } //  if (level <= debugging_level)
} // debug(int level, String msg, int value)


// +++++++++++++++++++++++++++++++       run_target_LED_test()       ++++++++++++++++++++++++++++
void run_target_LED_test() {
 
  for (int tile = 1; tile <= number_of_tiles; tile++) {
    light_target_strip(tile, try_to_hit_code);
    delay(1000);
    light_target_strip(tile, hit_code);
    delay(1000);
    light_target_strip(tile, miss_code);
    delay(1000);
  } // (int tile = 1; tile <= number_of_tiles; tiles++) 

} // debug(int level, String msg, int value)


