// for interfacing with cpu controlling camera
// has the GPIO to trigger camera and accepts commands

#include "Config.h"
#include "Sensors.h"
//#include "SystemTrigger.h"
#include "Utils.h"
#include "LEDring.h"
#include "AD5272.h"
#include "ReadInput.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, CAM_ENABLE, &io, 8); // led ring object
//LEDring leds(STROBE_TRIG, &io, 8); // led ring object - test it's ok if no CAM_ENABLE pin
LEDring leds(STROBE_TRIG, CAM_ENABLE, CAM_READY, &io, 8); // led ring object
Sensors sensors;
AD5272 currentSrc(AD5272_ADDR);

int LEDnum, duration_ms, result, cmdNum;

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

    // Start the ports
    DEBUGPORT.begin(115200);
    HWPORT0.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 systemcontrol.begin
    sensors.begin();

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

}

void loop() {

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

    result = readLEDcmd(&UI1, &LEDnum, &duration_ms, &cmdNum);
    //UI1.printf("Return value %d\n\r",result);

    if (result == 1) { // success!
        if (cmdNum == 1) {
            leds.strobeLED(LEDnum, duration_ms);
        } else if (cmdNum == 2) { // continuous mode
            leds.LEDon_strobeCam(LEDnum, duration_ms, &UI1);
        }
    }

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