#include "s0.h"

S0Class S0;

void S0Class::begin (uint8_t _pin) {
  pin = _pin;
  pinMode(pin,OUTPUT);
  digitalWrite(pin,LOW);
  pinMode(13,OUTPUT);
  digitalWrite(13,LOW);
  count = 0;
  state = INIT;
}
void S0Class::tick () {
  unsigned long newms = millis();
  if (state==INIT) {
    if (FROMNET && SOLAR && INTONET) {
      count++;
      ms = newms;
      fromnet = FROMNET;
      intonet = INTONET;
      kwh = fromnet+SOLAR-intonet;
      pulses = 0;
      high = false;
      state = START;
    }
  } else if (newms>ms+RESYNC) {
    count++;
    if (FROMNET && SOLAR && INTONET) {
      unsigned long newkwh;
      ms = newms;
      int powerpulses = PowerSerial::power.light.pulses();
      int solarpulses = PowerSerial::solar.light.pulses();
      if (FROMNET==fromnet && INTONET>intonet) {
        // We feed the network. Power pulses are negative. House consumes solely solar.
        powerpulses = -powerpulses;
      } else if (FROMNET>fromnet && INTONET==intonet) {
        // We consume from network in addition to solar (if any).
      } else {
        // We can't decide the flow direction of the power pulses. Disregard them.
        powerpulses = 0;
      }
      fromnet = FROMNET;
      intonet = INTONET;
      newkwh = fromnet+SOLAR-intonet+powerpulses+solarpulses;
      pulses += newkwh-kwh;
      kwh = newkwh;
      msdelta = pulses>0? RESYNC/pulses: 0;
      if (state==START) {
        pulsems = ms;
        state = SYNC;
      }
    } else {
      state = INIT;
    }
  }
  if (pulses>=PULSEFACTOR && newms>=pulsems) {
    high = !high;
    digitalWrite(pin,high?HIGH:LOW);
    digitalWrite(13,high?HIGH:LOW);
    long pulsedelta = high? PULSEMS: msdelta*PULSEFACTOR-PULSEMS;
    // pulsems += high?PULSEMS:msdelta*PULSEFACTOR-PULSEMS;
    pulsems = newms + pulsedelta;
    if (!high) pulses-=PULSEFACTOR;
  }
}