// measure LED - frozen as of 4/12/23
// has some code for measuring LED current and voltage

#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

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, 8); // led ring object
Sensors sensors;
AD5272 currentSrc(AD5272_ADDR);

int ledCounter = 0;

void scanI2C(); // declare function i define below

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

    //scanI2C();

    // 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 systemcontrol.begin
    sensors.begin();

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

}

void loop() {

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

    //leds.strobeLED(ledCounter % 8, 100);

    // to test the pack LED functions, call that instead
    
    byte tester = 0b01 << (ledCounter%8);
    //leds.strobeMultLEDs(tester, 100);
    //leds.strobeMultLEDs_trigPwrMeas(tester, 40, &sensors, INA260_TIME_1_1_ms, INA260_COUNT_16);
    leds.strobeMultLEDs_trigPwrMeas(tester, 4, &sensors, INA260_TIME_1_1_ms, INA260_COUNT_1);
    //leds.strobeMultLEDs_trigPwrMeas(tester, 1200, &sensors, INA260_TIME_1_1_ms, INA260_COUNT_512);
    UI1.print(tester, BIN);
    UI1.print(",");
    sensors.printPwr1(); // function should also read them in
    UI1.print(tester, BIN);
    UI1.print(",");
    sensors.printPwr2();

    // now let's test turning them on in packs of two. or three.
    /*
    byte tester = 0b01 << (ledCounter%8);
    tester |= 0b01 << ((ledCounter+1)%8); 
    //tester |= 0b01 << ((ledCounter+2)%8);
    //leds.strobeMultLEDs(tester, 100);
    leds.strobeMultLEDs_trigPwrMeas(tester, 40, &sensors, INA260_TIME_1_1_ms, INA260_COUNT_16);
    UI1.print(tester, BIN);
    UI1.print(",");
    sensors.printPwr1(); // function should also read them in
    UI1.print(tester, BIN);
    UI1.print(",");
    sensors.printPwr2();*/

    // let's turn all on together and see what happens
    //leds.strobeMultLEDs(255, 100);
    /*
    //leds.strobeMultLEDs_trigPwrMeas(255, 40, &sensors, INA260_TIME_1_1_ms, INA260_COUNT_16);
    leds.strobeMultLEDs_trigPwrMeas(255, 4, &sensors, INA260_TIME_1_1_ms, INA260_COUNT_1);
    UI1.print(255, BIN);
    UI1.print(",");
    sensors.printPwr1(); // function should also read them in
    UI1.print(255, BIN);
    UI1.print(",");
    sensors.printPwr2();*/

    ledCounter++;

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

    if (ledCounter % 8 == 0) { // let's update and display our env variables, right now is I2C check
        //sensors.update(); // don't need to update because it always updates when it prints anyways
        //sensors.printEnv();
        sensors.triggerPower(); 
        // get a passive power draw number; use the same settings as for the other ones, for now
        // we'll stay in one-shot mode
        delayMicroseconds(40*1000);
        UI1.print("0,");
        sensors.printPwr1(); // function should also read them in
        UI1.print("0,");
        sensors.printPwr2();

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

        //scanI2C();
    }
}

void scanI2C()
// scan I2C bus - runnign into issues
{
	byte error, address;
	int nDevices;

    Wire.begin();
 
	DEBUGPORT.println("Scanning...");

	nDevices = 0;
	for(address = 1; address < 127; address++ )
	{
        // The i2c_scanner uses the return value of
        // the Write.endTransmisstion to see if
        // a device did acknowledge to the address.
        Wire.beginTransmission(address);
        error = Wire.endTransmission();

        if (error == 0)
        {
            DEBUGPORT.print("I2C device found at address 0x");
            if (address<16)
            DEBUGPORT.print("0");
            DEBUGPORT.print(address,HEX);
            DEBUGPORT.println("  !");

            nDevices++;
        }
        else if (error==4)
        {
            DEBUGPORT.print("Unknown error at address 0x");
            if (address<16)
            DEBUGPORT.print("0");
            DEBUGPORT.println(address,HEX);
        } 

        //delayMicroseconds(10000);   
	}
	if (nDevices == 0)
		DEBUGPORT.println("No I2C devices found\n");
	else
		DEBUGPORT.println("done\n");
}