#include <LightChrono.h>
#include <Chrono.h>

const float APPARENT_SHUNT_RESISTANCE = 0.453;
const float VOLTAGE_REFERENCE = 2.494;

float ampseconds = 0;
float wattseconds = 0;

const int digits[] = {2, 3, 4, 5};
const int segments[] = {6, 7, 8, 9, 10, 11, 12, 13};

const bool one[7] = {false, true, true, false, false, false, false};
const bool two[7] = {true, true, false, true, true, false, true};
const bool three[7] = {true, true, true, true, false, false, true};
const bool four[7] = {false, true, true, false, false, true, true};
const bool five[7] = {true, false, true, true, false, true, true};
const bool six[7] = {true, false, true, true, true, true, true};
const bool seven[7] = {true, true, true, false, false, false, false};
const bool eight[7] = {true, true, true, true, true, true, true};
const bool nine[7] = {true, true, true, true, false, true, true};
const bool zero[7] = {true, true, true, true, true, true, false};

const bool U[7] = {false, true, true, true, true, true, false};
const bool A[7] = {true, true, true, false, true, true, true};
const bool E[7] = {true, false, false, true, true, true, true};

unsigned char display_state = 0;

Chrono timer;
Chrono debounce;
Chrono blank_timer;

float shunt_voltage = 0;
float shunt_current = 0;
float battery_voltage = 0;
float amps_in_period = 0;
float watts_in_period = 0;
float mamphours = 0;
float mwatthours = 0;


void writeDigit(char d, char val, bool dec) {
    const bool *segs;
    switch (val) {
        case 0:
            segs = zero;
            break;
        case 1:
            segs = one;
            break;
        case 2:
            segs = two;
            break;
        case 3:
            segs = three;
            break;
        case 4:
            segs = four;
            break;
        case 5:
            segs = five;
            break;
        case 6:
            segs = six;
            break;
        case 7:
            segs = seven;
            break;
        case 8:
            segs = eight;
            break;
        case 9:
            segs = nine;
            break;
    }
    for (int i = 0; i < 7; ++i) {
        if (segs[i]) {
            digitalWrite(segments[i], HIGH);
        } else {
            digitalWrite(segments[i], LOW);
        }
    }
    if (dec) {
      digitalWrite(segments[7], HIGH);
    }
    digitalWrite(digits[d], LOW);
    delay(1);
    digitalWrite(digits[d], HIGH);
    digitalWrite(segments[7], LOW);
}

void writeLetter(char d, const bool val[]) {
    for (int i = 0; i < 7; ++i) {
        if (val[i]) {
            digitalWrite(segments[i], HIGH);
        } else {
            digitalWrite(segments[i], LOW);
        }
    }
    digitalWrite(digits[d], LOW);
    delay(1);
    digitalWrite(digits[d], HIGH);
}

void showNumber(float number) {
    int n = number;
    writeDigit(0, (n/1000U) % 10, false);
    writeDigit(1, (n/100U) % 10, false);
    writeDigit(2, (n/10U) % 10, false);
    writeDigit(3, n % 10, false);
}

void showVoltage(float volts) {
    int cvolts = volts * 100;
    writeDigit(0, (cvolts/100U) % 10, true);
    writeDigit(1, (cvolts/10U) % 10, false);
    writeDigit(2, cvolts % 10, false);
    writeLetter(3, U);
}

void showCurrent(float current) {
    int camp = current * 100;
    writeDigit(0, (camp/100U) % 10, true);
    writeDigit(1, (camp/10U) % 10, false);
    writeDigit(2, camp % 10, false);
    writeLetter(3, A);
}

void showWh(float watthours) {
  int w = watthours;
  writeDigit(0, (w/1000U) % 10, true);
  writeDigit(1, (w/100U) % 10, false);
  writeDigit(2, (w/10U) % 10, false);
  writeLetter(3, E);
}


void setup() {
    // put your setup code here, to run once:
    Serial.begin(9600);
    analogReference(EXTERNAL);

    for (int i = 0; i < 4; ++i) {
        pinMode(digits[i], OUTPUT);
        digitalWrite(digits[i], HIGH);
    }
    for (int i = 0; i < 8; ++i) {
        pinMode(segments[i], OUTPUT);
        digitalWrite(segments[i], LOW);
    }

    pinMode(15, OUTPUT);
    digitalWrite(15, LOW);

    pinMode(19, INPUT_PULLUP);
    pinMode(20, INPUT_PULLUP);
    pinMode(21, INPUT_PULLUP);
}

void loop() {
    

    if (timer.hasPassed(1000)) {
        unsigned long passed = timer.elapsed();
        timer.restart();
        battery_voltage = readVoltage(A0);

        if (battery_voltage < 0.8) {
            blank_timer.restart();
        } else {
            shunt_voltage = readVoltage(A1);
            shunt_current = shunt_voltage/APPARENT_SHUNT_RESISTANCE;


            amps_in_period = shunt_current * passed/1000;
            watts_in_period = amps_in_period * battery_voltage;

            ampseconds += amps_in_period;
            wattseconds += watts_in_period;
            mamphours = ampseconds/3.6;
            mwatthours = wattseconds/3.6;
        }
    }
    if (debounce.hasPassed(300) && !digitalRead(19)) {
        display_state = (display_state + 1) % 4;
        debounce.restart();
    } else if (blank_timer.hasPassed(200)) {
        switch (display_state) {
          case 0:
            showNumber(mamphours);
            break;
          case 1:
            showVoltage(battery_voltage);
            break;
          case 2:
            showCurrent(shunt_current);
            break;
          case 3:
            showWh(mwatthours);
            break;
        }
    }
}

float readVoltage(int pin) {

    int val = 0;
    for (int i = 0; i < 4; ++i) {
        val = val + analogRead(pin);
        delay(0.5);
    }

    return (val / 4) * VOLTAGE_REFERENCE/1023.0;
}