// PWM-style light source for all LEDs - last update 4/14/23
// also contains code for testing overhead timing for function calls
// implementation of light source uses this timing info to calculate 
// duration to set per LED (bundle) given frequency

#include "Config.h"
#include "Sensors.h"
#include "SystemTrigger.h"
#include "Utils.h"
#include "LEDring.h"
#include "AD5272.h"

#include "../lib/SparkFun_SX1509_Arduino_Library/src/SparkFunSX1509.h"

#include <Adafruit_NeoPixel.h>

//#define PIN_NEOPIXEL 11 // already defined in Config.h

#define STROBEFREQ_HZ 50
#define NUMLEDS 8
#define OVERHEAD_MS 10

SX1509 io; // SX1509 object to be used throughout
Adafruit_NeoPixel pixels(1, PIN_NEOPIXEL); // pixel strand with 1 pixel on PIN_NEOPIXEL
LEDring leds(STROBE_TRIG, &io, NUMLEDS); // led ring object
Sensors sensors;
AD5272 currentSrc(AD5272_ADDR);

int ledCounter = 0;

// name High Mag is a holdover from code this was copied from
// right now is just a timer for setting off a sequence where all LEDs are turned on
// 4/13/13 we'll start with all even durations on

double durationPerLED_ms = ((1000/STROBEFREQ_HZ) - OVERHEAD_MS)/5; // number of BUNDLES is 5
//double durationPerLED_ms = 8; // this was for testing different speeds

void HighMagCallback()
{
    // bundled and hacked version - it's the fastest
    // 4/14/23
    io.digitalWrite(1,HIGH);
    io.digitalWrite(2,HIGH);
    leds.strobeLED_pinOnly(durationPerLED_ms);
    io.digitalWrite(1,LOW);
    io.digitalWrite(2,LOW);

    io.digitalWrite(3,HIGH);
    io.digitalWrite(7,HIGH);
    leds.strobeLED_pinOnly(durationPerLED_ms);
    io.digitalWrite(3,LOW);
    io.digitalWrite(7,LOW);

    leds.strobeLED(3, durationPerLED_ms);

    io.digitalWrite(5,HIGH);
    io.digitalWrite(6,HIGH);
    leds.strobeLED_pinOnly(durationPerLED_ms);
    io.digitalWrite(5,LOW);
    io.digitalWrite(6,LOW);

    leds.strobeLED(7, durationPerLED_ms);
}

void setup() {
    // Neopixel power
    pinMode(12, OUTPUT);
    digitalWrite(12, HIGH);

    // Start the debug port - for now we're using this for comms
    DEBUGPORT.begin(115200);
    delay(4000);

    pixels.begin();  // initialize the pixel

    // Setup the port expander
    if (!io.begin(SX1509_ADDRESS))
    {
        DEBUGPORT.println("Could not init SX1509.");
        while(true){};
    }

    // set up LED
    leds.init();

    // Start sensors - in Planktivore main, this is in sys.begin
    sensors.begin();

    // digital pot for current source - set to max
    currentSrc.begin();
    //currentSrc.setwiper(NUMPOS-1);
    currentSrc.setwiper(512); // this is default

    // set up timer frequqency - for the PWM light source
    // in Planktivore, this is in sys.setTriggers
    configTriggers(STROBEFREQ_HZ); 

    Wire.setClock(400000);

}

void loop() {

    // set the first pixel #0 to red
    //pixels.setPixelColor(0, pixels.Color(255, 0, 0));
    // and write the data
    //pixels.show();

    // turn off the pixel
    //pixels.clear();
    //pixels.show();
  
    delay(1000);

    //sensors.update(); // don't need to update because it always updates when it prints anyways
    // actually none of this gets called bc timing is too tight for the timer
    sensors.printEnv();
    sensors.printPower();

    // for timing
    /*
    int i;
    Serial.print(durationPerLED_ms);
    Serial.print(",");
    unsigned long starttime = micros(); // time my overhead

    /*
    for (i = 0; i < NUMLEDS; i++) {
        leds.strobeLED(i, durationPerLED_ms);
    }
    */

    // group some LEDs together
    /*
    leds.strobeMultLEDs(0b00000011, durationPerLED_ms); // 1 and 2
    leds.strobeMultLEDs(0b01000100, durationPerLED_ms); // 3 and 7
    leds.strobeLED(3, durationPerLED_ms); // 4 (the function takes 0 through 7, so subtract 1 from LED#)
    leds.strobeMultLEDs(0b00110000, durationPerLED_ms); // 5 and 6
    leds.strobeLED(7, durationPerLED_ms); // 8
    */

    // hack the grouping to reduce overhead
    /*
    io.digitalWrite(1,HIGH);
    io.digitalWrite(2,HIGH);
    leds.strobeLED_pinOnly(durationPerLED_ms);
    io.digitalWrite(1,LOW);
    io.digitalWrite(2,LOW);

    io.digitalWrite(3,HIGH);
    io.digitalWrite(7,HIGH);
    leds.strobeLED_pinOnly(durationPerLED_ms);
    io.digitalWrite(3,LOW);
    io.digitalWrite(7,LOW);

    leds.strobeLED(3, durationPerLED_ms);

    io.digitalWrite(5,HIGH);
    io.digitalWrite(6,HIGH);
    leds.strobeLED_pinOnly(durationPerLED_ms);
    io.digitalWrite(5,LOW);
    io.digitalWrite(6,LOW);

    leds.strobeLED(7, durationPerLED_ms);
    */

    //DEBUGPORT.println(micros() - starttime);
    /*DEBUGPORT.print("us elapsed with ");
    DEBUGPORT.print(NUMLEDS);
    DEBUGPORT.print("LEDs x ");
    DEBUGPORT.print(durationPerLED_ms);
    DEBUGPORT.println("ms each.");*/

    //UI1.println(currentSrc.readwiper());
}