#ifndef _SYSTEMCONTROL

#define _SYSTEMCONTROL

#include <Arduino.h>
#include <RTCZero.h>
#include <RTClib.h>
#include <WDTZero.h>
#include "Config.h"
#include "DeepSleep.h"
#include "SPIFlash.h"
#include "Sensors.h"
#include "PortExpander.h"
#include "CommsExpander.h"
#include "PortControl.h"
#include "Stats.h"
#include "SystemConfig.h"
#include "SystemTrigger.h"
#include "Utils.h"
#include "Test.h"
#include "PCF2131_I2C.h"

#define CMD_CHAR '!'
#define PROMPT "SSC > "
#define LOG_PROMPT "$SSC"
#define CMD_BUFFER_SIZE 128

// Global Sensors
Sensors _sensors;

// Global RTCZero
RTCZero _zerortc;

//Global RTCLib
#ifdef RTC_PCF
    PCF2131_I2C _pcfRTC;
#else RTC_DS3231
    RTC_DS3231 _ds3231;
#endif

// Global watchdog timer with 8 second hardware timeout
WDTZero _watchdog;

// Port Expander
PortExpander _portExpander;

// RS232 comms expander
CommsExpander _commsExpander;

// Port controller
PortControl _portControl;

// Test lib
SCCTest _scctest;

// Static polling function for instruments
int instrumentType = 0;
bool pollingEnable = false;
void pollInstruments() {
    if (!pollingEnable)
        return;
    switch (instrumentType) {
        case 0:
            break;
        case 1:
            break;
        default:
            break;
    }
}

class SystemControl
{
    private:
    bool systemOkay;
    bool ds3231Okay;
    bool cameraOn;
    bool bmeOn;
    bool boostEnable;
    bool pendingPowerOff;
    bool pendingPowerOn;
    bool lowVoltage;
    bool badEnv;
    char cmdBuffer[CMD_BUFFER_SIZE];
    int state;
    unsigned long timestamp;
    unsigned long startupTimer;
    unsigned long lastPowerOnTime;
    unsigned long lastPowerOffTime;
    unsigned long pendingPowerOffTimer;
    unsigned long pendingPowerOnTimer;
    unsigned long clockSyncTimer;
    unsigned long envTimer;
    unsigned long voltageTimer;

    int lastFlashType, lastLowMagDuration, lastHighMagDuration, lastFrameRate;

    MovingAverage<float> avgVoltage;
    MovingAverage<float> avgTemp;
    MovingAverage<float> avgHum;

    void readInput(Stream *in) {

        if (in != NULL && in->available() > 0) {
            char c = in->read();
            if (c == CMD_CHAR) {

                // Don't echo the command char
                //if (cfg.getInt("LOCALECHO"))
                //    in->write(c);

                // Print the prompt
                in->write(PROMPT);


                unsigned long startTimer = millis();
                int index = 0;
                while (startTimer <= millis() && millis() - startTimer < (unsigned int)(cfg.getInt("CMDTIMEOUT"))) {

                    // Break if we have exceed the buffer size
                    if (index >= CMD_BUFFER_SIZE)
                        break;

                    // Wait on user input
                    if (in->available()) {
                        // Read the next char and reset timer
                        c = in->read();
                        startTimer = millis();
                    }
                    else {
                        continue;
                    }

                    // Exit command loop on repeat command char
                    if (c == CMD_CHAR) {
                        break;
                    }

                    if (c == '\r') {
                        // Command ended try to
                        if (index <= CMD_BUFFER_SIZE)
                            cmdBuffer[index++] = '\0';
                        else
                            cmdBuffer[CMD_BUFFER_SIZE-1] = '\0';

                        // Parse Command menus
                        char * rest;
                        char * cmd = strtok_r(cmdBuffer,",",&rest);

                        // CFG (configuration commands)
                        if (cmd != NULL && strncmp_ci(cmd,CFG, 3) == 0) {
                            if (rest != NULL) {
                                cfg.parseConfigCommand(rest, in);
                            }
                            else {
                                cfg.printConfig(in);

                            }

                        }

                        // PORTPASS (pass through to other serial ports)
                        if (cmd != NULL && strncmp_ci(cmd,PORTPASS, 8) == 0) {
                            doPortPass(in, rest);
                        }

                        // SETTIME (set time from string)
                        if (cmd != NULL && strncmp_ci(cmd,SETTIME, 7) == 0) {
                            setTime(rest, in);
                        }

                        // WRITECONFIG (save the current config to EEPROM)
                        if (cmd != NULL && strncmp_ci(cmd,WRITECONFIG, 11) == 0) {
                            writeConfig();
                        }

                        // READCONFIG (read the current config to EEPROM)
                        if (cmd != NULL && strncmp_ci(cmd,READCONFIG, 10) == 0) {
                            readConfig();
                        }

                        if (cmd != NULL && strncmp_ci(cmd,TRIGENABLE,11) == 0) {
                            triggerEnabled = true;
                        }

                        if (cmd != NULL && strncmp_ci(cmd,TRIGDISABLE,12) == 0) {
                            triggerEnabled = false;
                        }

                        if (cmd != NULL && strncmp_ci(cmd,CAMERAON,8) == 0) {
                            turnOnSystem();

                        }

                        if (cmd != NULL && strncmp_ci(cmd,CAMERAOFF,9) == 0) {
                            turnOffSystem();
                        }

                        if (cmd != NULL && strncmp_ci(cmd,RUNTESTS,8) == 0) {
                            // Print out port values
                            for (int i=0 ; i< 16;i++) {
                                DEBUGPORT.println(_portExpander.readPort(i));
                                delay(100);
                            }
                            //_scctest.test_interactively();
                        }

                        if (cmd != NULL && strncmp_ci(cmd,READCURRENT,11) == 0) {
                            in->println();
                            for (int p = 0; p < 8; p++) {
                                in->println(_portControl.readPortCurrent(p));
                            }
                        }

                        // Reset the buffer and print out the prompt
                        if (c == '\n')
                            in->write('\r');
                        else
                            in->write("\r\n");

                        in->write(PROMPT);

                        index = 0;
                        startTimer = millis();
                        continue;
                    }

                    // Handle backspace
                    if (c == '\b') {
                        index -= 1;
                        if (index < 0) {
                            index = 0;
                        }
                        else if ( index >= 0 && cfg.getInt(LOCALECHO)) {
                           in->write("\b \b");
                        }
                    }
                    else {
                        cmdBuffer[index++] = c;
                        if (cfg.getInt(LOCALECHO))
                            in->write(c);
                    }
                }
            }
        }
    }

    void doPortPass(Stream * in, char * cmd) {
        char * rest;
        char * num = strtok_r(cmd,",",&rest);
        in->print("Passing through to hardware port ");
        in->println(num);
        in->println();
        if (num != NULL) {
            int portNumber = atoi(num);
            _commsExpander.selectPort(portNumber);
            portpass(in, &SENSOR_PORT, cfg.getInt(LOCALECHO) == 1);
        }
    }

    void setTime(char * timeString, Stream * ui) {
        struct tm now_tm;
        if (timeString != NULL) {
            // if we have ds3231 set that first
            DateTime dt(timeString);
            if (dt.isValid()) {
                ui->println("\nUpdating clock...\n");
                if (ds3231Okay) {
#ifdef RTC_PCF
                    ui->println("Using PCF2131...");
                    now_tm.tm_year = dt.year() - 1900;
                    now_tm.tm_mon = dt.month() - 1;  // It needs to be '3' if April
                    now_tm.tm_mday = dt.day();
                    now_tm.tm_hour = dt.hour();
                    now_tm.tm_min = dt.minute();
                    now_tm.tm_sec = dt.second();
                    _pcfRTC.set(&now_tm);
#else
                    _ds3231.adjust(dt.unixtime());
#endif
                }
                _zerortc.setEpoch(dt.unixtime());
            }
        }
    }

    void getTimeString(char * timeString) {
        
        sprintf(timeString,"%s","YYYY-MM-DD hh:mm:ss");
        unsigned long unixtime;
        DateTime now;
        if (ds3231Okay) {
#ifdef RTC_PCF
            time_t current_time = 0;
            current_time = _pcfRTC.time(NULL);
            now = current_time;
            //sprintf(timeString,"%s",ctime(&current_time));
#else
            now = _ds3231.now();
            unixtime = now.unixtime();
#endif
            now.toString(timeString);
        }
        else {
            unixtime = _zerortc.getEpoch();
            sprintf(timeString, "%04d-%02d-%02d %02d:%02d:%02d", 
                _zerortc.getYear(),
                _zerortc.getMonth(),
                _zerortc.getDay(),
                _zerortc.getHours(),
                _zerortc.getMinutes(),
                _zerortc.getSeconds()
            );
        }
    }


    public:

    SystemConfig cfg;
    bool triggerEnabled;
    int trigWidth;
    int lowMagStrobeDuration;
    int highMagStrobeDuration;
    int flashType;
    int frameRate;

    SystemControl() {
        systemOkay = false;
        state = 0;
        timestamp = 0;
        ds3231Okay = false;
        pendingPowerOff = false;
        cameraOn = false;
        boostEnable = false;
        triggerEnabled = false;
        lowVoltage = false;
        badEnv = false;
    }

    bool begin() {

        // Output Pins
        configOutputPin(V12_ON,LOW); 
        configOutputPin(ANODE_EN,LOW);
        configOutputPin(ORIN_STRB_OUT,LOW);
        configOutputPin(RS232_EN,LOW); 
        configOutputPin(ADC_SELECT_0,LOW); 
        configOutputPin(ADC_SELECT_1,LOW); 
        configOutputPin(ADC_SELECT_2,LOW); 
        configOutputPin(RS232_SELECT_0,LOW); 
        configOutputPin(RS232_SELECT_1,LOW); 
        configOutputPin(RS232_SELECT_2,LOW);  
        configOutputPin(GPIO_0_RESET,LOW);  
        configOutputPin(BME280OFF,LOW); 
        configOutputPin(ETHERNET_ON,LOW);  

        // Input Pins
        configInputPin(ORIN_STRB_IN); 
        //configInputPin(PORT_ISNS_AIN_0);  
        //configInputPin(ISO_SNS_AIN_1); 
        //configInputPin(SCC_VREF); 
        configInputPin(SMBALERT); 
        configInputPin(GPIO_0_INT); 
    


        delay(1000);

        // Start RTC
        _zerortc.begin();

        // Init port expander
        _portExpander.initialize();

        // Start DS3231
#ifdef RTC_PCF
        time_t current_time = 0;
        current_time = _pcfRTC.time(NULL);
        // sync rtczero to DS3231
        DEBUGPORT.println("PCF2131 init OK.");
        _zerortc.setEpoch(current_time);
        ds3231Okay = true;
#else
        if (!_ds3231.begin()) {
            DEBUGPORT.println("Could not init DS3231, time will be lost on power cycle.");
            ds3231Okay = false;
        } else {
            // sync rtczero to DS3231
            DEBUGPORT.println("DS3231 init OK.");
            _zerortc.setEpoch(_ds3231.now().unixtime());
            ds3231Okay = true;
        }
#endif

        // set the startup timer
        startupTimer = _zerortc.getEpoch();
        lastPowerOffTime = _zerortc.getEpoch();
        lastPowerOnTime = _zerortc.getEpoch();
        voltageTimer = _zerortc.getEpoch();
        envTimer = _zerortc.getEpoch();
        clockSyncTimer = _zerortc.getEpoch();

        systemOkay = true;   // Not sure if this is ever set to false anywhere besdies constructor
        if (_flash.initialize()) {
            DEBUGPORT.println("Flash Init OK.");
        } else {
            DEBUGPORT.print("Init FAIL, expectedDeviceID(0x");
            DEBUGPORT.print(_expectedDeviceID, HEX);
            DEBUGPORT.print(") mismatched the read value: 0x");
            DEBUGPORT.println(_flash.readDeviceId(), HEX);
        }

        // Start sensors
        bmeSwitch(true);
        _sensors.begin();

        return true;

    }

    void storeLastFlashConfig() {
        // Set last config in case we call end event before start event
        lastFlashType = cfg.getInt(FLASHTYPE);
        lastFrameRate = cfg.getInt(FRAMERATE);
        lastHighMagDuration= cfg.getInt(FLASHDUR);
    }

    void restoreLastFlashConfig() {
        cfg.set(FLASHTYPE, lastFlashType);
        cfg.set(FRAMERATE, lastFrameRate);
        cfg.set(FLASHDUR, lastHighMagDuration);
    }

    void configWatchdog() {
        // enable hardware watchdog if requested
        if (cfg.getInt(WATCHDOG) > 0) {
            _watchdog.setup(WDT_HARDCYCLE8S);
        }
    }

    // BME280 pin driven low (LOW == on)
    void bmeSwitch(bool turn_on) {
        pinMode(BME280OFF, OUTPUT);
        if (turn_on) {
            digitalWrite(BME280OFF, LOW);
        } else {
            digitalWrite(BME280OFF, HIGH);
        }
        bmeOn = turn_on;
    }

    // Turn on 12V
    void turnOn12V() {
        digitalWrite(V12_ON, HIGH);
        cameraOn = true;
    }

    // Turn on Ethernet switch
    void turnOnEth() {
        digitalWrite(ETHERNET_ON, HIGH);
    }

    // Turn off Ethernet switch
    void turnOffEth() {
        digitalWrite(ETHERNET_ON, LOW);
    }

    // Turn on Orin
    void turnOnOrin() {
        _portExpander.enablePort(15);
    }

    // Turn off Orin
    void turnOffOrin() {
        _portExpander.disablePort(15);
    }

    // Turn off 12V
    void turnOff12V() {
        digitalWrite(V12_ON, LOW);
        cameraOn = false;
    }

    void turnOnAllPorts() {
        _portExpander.enablePort(0); // Port 1
        _portExpander.enablePort(2); // Port 2
        _portExpander.enablePort(4); // Port 3
        _portExpander.enablePort(6); // Port 4
        _portExpander.enablePort(9); // Port 5
        _portExpander.enablePort(11); // Flow
        _portExpander.enablePort(13); // Modem
    }

    void turnOffAllPorts() {
        _portExpander.disablePort(0); // Port 1
        _portExpander.disablePort(2); // Port 2
        _portExpander.disablePort(4); // Port 3
        _portExpander.disablePort(6); // Port 4
        _portExpander.disablePort(9); // Port 5
        _portExpander.disablePort(11); // Flow
        _portExpander.disablePort(13); // Modem
    }

    void turnOnSystem() {
        turnOn12V();
        delay(500);
        turnOnEth();
        delay(100);
        turnOnOrin();
        turnOnAllPorts();
    }

    void turnOffSystem() {
        turnOff12V();
        delay(500);
        turnOffEth();
        delay(100);
        turnOffOrin();
        turnOffAllPorts();
    }

    bool update() {

        // Run updates and check for new data
        _sensors.update();

        char timeString[64];
        getTimeString(timeString);

        // Build log string and send to UIs
        char output[256];

        // The system log string, note this requires enabling printf_float build
        // option work show any output for floating point values
        sprintf(output, "%s,%s.%03u,%0.3f,%0.3f,%0.3f,%0.2f,%d,%d,%d,%d,%d",

            LOG_PROMPT,
            timeString,
            ((unsigned int) millis()) % 1000,
            _sensors.temperature, // In C
            _sensors.pressure / 1000, // in kPa
            _sensors.humidity, // in %
            (int)cameraOn,
            (int)boostEnable,
            (int)triggerEnabled,
            highMagStrobeDuration,
            frameRate
        );

        // Send output
        printAllPorts(output);

        return true;
    }

    void writeConfig() {
        if (systemOkay) {
            cfg.writeConfig();
        }
    }

    void readConfig() {
        if (systemOkay)
            cfg.readConfig();
    }

    void checkInput() {
        if (DEBUGPORT.available() > 0) {
            readInput(&DEBUGPORT);
        }
        if (UI1.available() > 0) {
            readInput(&UI1);
        }
        if (UI2.available() > 0) {
            readInput(&UI2);
        }
    }

    void printAllPorts(const char output[]) {
        DEBUGPORT.println(output);
        UI1.println(output);
        UI2.println(output);
    }

    void checkEnv() {
        if (_zerortc.getEpoch() - startupTimer <= (unsigned int)cfg.getInt(STARTUPTIME))
            return;


        // Update moving average of temperature
        float latestTemp = avgTemp.update(_sensors.temperature);
        float latestHum = avgHum.update(_sensors.humidity);
        //float latestVoltage = _sensors.voltage[0];

        // Make sure this check happens AFTER updating the average measurement, otherwise
        // the average will not be calculated properly
        if (_zerortc.getEpoch() - envTimer <= (unsigned int)cfg.getInt(CHECKINTERVAL))
            return;

        // Reset check timer
        envTimer = _zerortc.getEpoch();

        if (latestTemp > cfg.getInt(TEMPLIMIT)) {
            char output[64];
            sprintf(output,"Temperature %0.2f C exceeds limit of %0.2f C", latestTemp, (float)cfg.getInt(TEMPLIMIT));
            printAllPorts(output);
            badEnv = true;
        }

        if (latestHum > cfg.getInt(HUMLIMIT)) {
            char output[64];
            sprintf(output,"Humidity %0.2f %% exceeds limit of %0.2f %%", latestHum, (float)cfg.getInt(HUMLIMIT));
            printAllPorts(output);
            badEnv = true;
        }

        badEnv = false;

    }

    void configureFlashDurations() {
        // Set global delays for ISRs
        trigWidth = cfg.getInt(TRIGWIDTH);
        highMagStrobeDuration = cfg.getInt(FLASHDUR);
    }

    void setTriggers() {
        frameRate = cfg.getInt(FRAMERATE);
        configTriggers(cfg.getInt(FRAMERATE));
    }

    void setPolling() {
        pollingEnable = true;
        configPolling(cfg.getInt(POLLFREQ), pollInstruments);
    }

    void setCTDType() {
        instrumentType = cfg.getInt(CTDTYPE);
    }

};

#endif