/*
 * LEDring.h
 * Interface with the LEDring via its specified pins and the SX1509 port expander
 *
 *  Created on: Mar 28, 2023
 *      Author: Irene Hu
 */

#include "LEDring.h"
#include "Config.h" // for debugport

//******************************************************************************
//* Constructors
//******************************************************************************

LEDring::LEDring(int pin_strobe, SX1509 *io, int numLEDs)
// externally declared and initiated SX1509 object is passed as pointer
{
    _pin_strobe = pin_strobe;
    _pin_cam = NULL;
    _pin_cam_ready = NULL;
    _io = io;
    _numLEDs = numLEDs;
}

LEDring::LEDring(int pin_strobe, int pin_cam, SX1509 *io, int numLEDs)
// externally declared and initiated SX1509 object is passed as pointer
// this version also has a camera trigger pin
// put it second in line to avoid ambiguities since numLEDs has a default value
{
    _pin_strobe = pin_strobe;
    _pin_cam = pin_cam;
    _pin_cam_ready = NULL;
    _io = io;
    _numLEDs = numLEDs;
}

LEDring::LEDring(int pin_strobe, int pin_cam, int pin_cam_ready, SX1509 *io, int numLEDs)
// externally declared and initiated SX1509 object is passed as pointer
// this version has both camera trigger and camera trigger_ready pins
{
    _pin_strobe = pin_strobe;
    _pin_cam = pin_cam;
    _pin_cam_ready = pin_cam_ready;
    _io = io;
    _numLEDs = numLEDs;
}


//******************************************************************************
//* Public Methods
//******************************************************************************

int LEDring::init()
// setup function for LED ring, sets all relevant pins to output and low
// use init() to check the SX1509 was set up correctly, returns 1 if success, 0 if not
{
    pinMode(_pin_strobe, OUTPUT);
    digitalWrite(_pin_strobe, LOW);

    if (_pin_cam) {
        pinMode(_pin_cam, OUTPUT);
        digitalWrite(_pin_cam, LOW);
    }

    if (_pin_cam_ready) {
        pinMode(_pin_cam_ready, INPUT);
    }

    // Check port expander
    // init() is supposedly deprecated, but it's called by begin() w/o setting variables
    // seems to just initialize Wire (ok to initialize multipel times?) and check comms
    if (!_io->init())
    {
        DEBUGPORT.println("Problem communicating with SX1509.");
        return 0;
    }

    // otherwise it was fine0
    // enable all LED-enable pins through SX1509 (can only assume it is initialized properly)
    for (int i=0; i < 15; i++) {
        _io->pinMode(i, OUTPUT);
        _io->digitalWrite(i,LOW);
    }

    // turn strobe on here and leave it on, to save time - this is just a test
    //digitalWrite(_pin_strobe, HIGH);

    return 1;
}


int LEDring::strobeLED(int LEDnum, double duration_ms)
// turn on LED specified by LEDnum for duration_ms ms
// if defined, also sets camera enable pin to HIGH, for triggering camera
// pin stays high for duration of strobe (ish), no offsets calculated
// returns 1 for success or 0 for fail (if specified LED is unavailable)
{
    if (LEDnum < 0 || LEDnum >= _numLEDs)
        return 0;

    _io->digitalWrite(LEDnum,HIGH);
    digitalWrite(_pin_strobe, HIGH);
    if (_pin_cam) digitalWrite(_pin_cam, HIGH);

    delayMicroseconds(duration_ms * 1000);

    if (_pin_cam) digitalWrite(_pin_cam, LOW);
    digitalWrite(_pin_strobe, LOW);
    _io->digitalWrite(LEDnum,LOW);
    
    return 1;
}


int LEDring::strobeLED_pinOnly(double duration_ms)
// turn on strobe pin for duration_ms ms
// hack version to speed things up, LEDs must be enabled and disabled separately
// returns 1 for success or 0 for fail (if specified LED is unavailable)
{
    digitalWrite(_pin_strobe, HIGH);

    delayMicroseconds(duration_ms * 1000);

    digitalWrite(_pin_strobe, LOW);
    
    return 1;
}


int LEDring::strobeMultLEDs(byte LEDnums, double duration_ms)
// turns on a specified group of LEDs for duration_ms ms - works for up to 8 LEDs
// specify LEDs to be enabled as bits of the LEDnums byte, LSB is LED1
{
    _enableMultLEDs(LEDnums);
    digitalWrite(_pin_strobe, HIGH);

    delayMicroseconds(duration_ms * 1000);

    digitalWrite(_pin_strobe, LOW);
    _disableAllLEDs();
    
    return 1;
}


int LEDring::strobeMultLEDs_trigPwrMeas(byte LEDnums, double duration_ms, Sensors* sensors, INA260_ConversionTime time, INA260_AveragingCount count)
// turns on a group of LEDs specified by bits of LEDnums byte (LSB = LED1), for duration_ms ms - works for up to 8 LEDs
// also triggers a power measurement right after turning strobe on
// specify the averaging count and conversion time (same for i and v for now), should be under duration_ms 
// for now, does not read, store, and restore previous state; ie permanently changes INA configs
// the LED-on timing is not exact for this because of time spent interfacinmg with INA260
{
    unsigned long starttime;
    sensors->setCurrentConversionTime(time);
    sensors->setVoltageConversionTime(time);
    sensors->setCurrentVoltageAvging(count);

    _enableMultLEDs(LEDnums);
    digitalWrite(_pin_strobe, HIGH);
    starttime = micros();
    sensors->triggerPower();

    while(micros() < starttime + (duration_ms * 1000)) ;
    //delayMicroseconds(duration_ms * 1000);

    if(!sensors->convReady_Power()) {
        DEBUGPORT.println("Sensor conversion was not ready after specified LED on time; waiting until ready");

        //int count = 0;
        
        while(!sensors->convReady_Power()) {
            //DEBUGPORT.print(count++);
            delayMicroseconds(10000); // otherwise it seems to hang here... maybe the I2C gets upset if keep pinging it too quickly
        } // for some reason this hangs - esp if the strobe time is too close to conversion time
        // i think there must be somethign like 10ms between calls to convReady, otherwise it won't return true
        //if(!sensors->convReady_Power()) { DEBUGPORT.println("Still nope."); }
        //DEBUGPORT.print("Time elapsed after ready: ");
        //DEBUGPORT.print((micros()-starttime)/1000);
        //DEBUGPORT.println("ms.");
    }

    //DEBUGPORT.print((micros()-starttime)/1000);
    //DEBUGPORT.println("ms.");

    digitalWrite(_pin_strobe, LOW);
    _disableAllLEDs();
    
    return 1;
}


int LEDring::LEDon_strobeCam(int LEDnum, double duration_ms, Stream * in)
// turn on LED, then strobes camera pin for duration_ms ms (exposure time)
// checks for cam_ready to go low before strobing again (so it will strobe as fast as camera can handle for this exposure time)
// exits upon receiving any character from specified input Stream
// returns 0 (fail) if LED ring isn't configured properly for cam or cam_ready pins (bc then what's the point?), or input stream not valid (can't exit!)
// returns 1 for success
{
    if (LEDnum < 0 || LEDnum >= _numLEDs)
        return 0;

    if (!_pin_cam || !_pin_cam_ready)
        return 0;

    if (in == NULL)
        return 0;

    // enable LED - then we will strobe it
    // otherwise it starts to flicker after awhile
    _io->digitalWrite(LEDnum,HIGH);

    // strobe loop
    while (in->available() <= 0) {
        if (digitalRead(_pin_cam_ready) == HIGH) {
            digitalWrite(_pin_strobe, HIGH);
            digitalWrite(_pin_cam, HIGH);
            delayMicroseconds(duration_ms * 1000);
            digitalWrite(_pin_cam, LOW);
            digitalWrite(_pin_strobe, LOW);
            delayMicroseconds(100000); // give camera a chance to respond?
        }
    }

    // some character received on stream, turn off LED
    digitalWrite(_pin_strobe, LOW);
    _io->digitalWrite(LEDnum,LOW);
    
    return 1;
}


//******************************************************************************
//* Private Methods
//******************************************************************************


int LEDring::_enableMultLEDs(byte LEDnums)
// enables a group of LEDs - works for up to 8 LEDs
// specify LEDs to be enabled as bits of the LEDnums byte, LSB is LED1
{
    int i, en;

    if (_numLEDs >= 8) {
        for (i = 0; i < 8; i++) {
            en = 0b01 & (LEDnums >> i);
            _io->digitalWrite(i,en);
            //if (en) {
            //    _io->digitalWrite(i,HIGH);
            //}
        }
    } else { // number of LEDs is less than 8, only check up to that number. any bits set past max LED# are ignored
        for (i = 0; i < _numLEDs; i++) {
            en = 0b01 & (LEDnums >> i);
            _io->digitalWrite(i,en);
            //if (en) {
            //    _io->digitalWrite(i,HIGH);
            //}
        }
    }

    return 1;
}


int LEDring::_disableAllLEDs(void)
// disables all LEDs
{
    int i;
    for (i = 0; i < _numLEDs; i++) {
        _io->digitalWrite(i,LOW);
    }

    return 1;
}