#include "MUX74HC4067.h"
#include <Wire.h>
#include <CmdMessenger.h>


#define WYP 0.63 // współczynnik wypływu
#define G 9.81 // przyśpieszenie ziemskie
#define TM 6 // time multipler
#define QM 11 // maksymalna wartość potencjometrów 1-6
#define X1 0.6 // poziom 1 czujnika wody
#define X2 1.3 // poziom 2 czujnika wody
#define X3 1.9 // poziom 3 czujnika wody
#define XT 90  // poziom czujki temperatury

byte x[14] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0 };
byte y[16] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 };

CmdMessenger cmdMessenger = CmdMessenger(Serial);

enum
{
  kAcknowledge,
  kError,
  kReceive,
  kSend,
};

class Tank {
  public:
    int x1 = 0, x2 = 0, x3 = 0, xT = 0; // czujki poziomu wody i temperatury
    double hT = 2; // wysokość zbiornika
    double r = 0.5; // promień podstawy
    double sT = PI * r * r; // pole podstawy
    double vT = sT * hT; // objętość maksymalna
    double hW = 0.0; // obecna wysokość wody
    double vW = 0; // obecna objętość wody
    double tWp = 20; // temperatura wody poprzednia
    double tWn = 20; // temperatura wody następna
    int m = 0; // mieszadło
    int overflowed = 0; // alarm przelania
    void check() {
      if (hW >= hT) {
        hW = 2; overflowed = 1;
      }
      else overflowed = 0;
      if (hW >= X1) x1 = 1;
      else x1 = 0;
      if (hW >= X2) x2 = 1;
      else x2 = 0;
      if (hW >= X3) x3 = 1;
      else x3 = 0;
      if (tWp >= XT) xT = 1;
      else xT = 0;
      vW = sT * hW;
    }
};
class Valve {
  public:
    int status = 0; // zamknięty/otwarty
    double r = 0.03; // promień zaworu
    double sV = PI * r * r; // powierzch max otworu
    double rSV = sV; // obecna powierzchnia otowru
    void qV(double x) {
      if (x != QM) rSV = sV / x;
      else rSV = 0;
    }
};
class Heater {
  public:
    int status = 0; // grzeje/nie grzeje
    int overheat = 0; // alarm przegrzania
    int p = 100; // moc grzałki w kW
    int oh = 0; // zmienna pomocnicza, timer przegrzania
    double alfa1 = 2.8; // współczynnik pierwszy do obliczeń temp
    double alfa2 = -0.8; // współczynnik drugi do obliczeń temp
    double alfa3 = 0.04; // współczynnik trzeci do obliczeń temp
    double tHp = 20; // temperatura grzałki poprzednia
    double tHn = 20; // temperatura grzałki obecna
    void check() {
      oh++;
      if (oh > 50) {
        overheat = 1;
        oh = 0;
      }
    }
    void dH(double x) {
      alfa2 = -x;
    }
};
void heat(Tank& t, Heater& h) {
  h.tHn = h.tHp + 0.1 * (h.p - h.alfa1 * (h.tHp - t.tWp));
  if (t.vW > 0)
    t.tWn =
      t.tWp +
      0.1 * (h.alfa2 * 1 / t.vW * (t.tWp - h.tHp) + h.alfa3 * (t.tWp - 20));
  else {
    t.check();
    t.tWn = 20;
  }
  if (t.tWn > 100) t.tWn = 100;
  t.tWp = t.tWn;
  h.tHp = h.tHn;
}
void cool(Tank& t, Heater& h) {
  h.oh = 0;
  t.tWn = 20 + (t.tWp - 20) * exp(-0.017 * 1 / t.vW);
  t.tWp = t.tWn;
  h.tHp = t.tWn;
}
void mixTemp(Tank& tP, Tank& tS, double d) {
  if (d > 0)
    tP.tWp = (tP.vW * 997 * tP.tWp + d * tS.sT * 997 * tS.tWp) /
             (tP.vW * 997 + d * tS.sT * 997);
}
double bind(Tank t, Valve v, int s, double h = 1000) {
  double d, q;
  if (v.rSV == 0) return 0;
  if (h == 1000) q = WYP * v.rSV * sqrt(2 * G * (h / 1000));
  else q = WYP * v.rSV * sqrt(2 * G * h);
  d = TM * q / t.sT;
  if (h < 1000)
    if (d >= h) d = h;
  if (d >= h) d = h;
  if (s == -1) return -d;
  return d;
}
void muxRead() {
  /*
  for (int i = 0; i < 10; ++i) {
    if (i < 6) data[i] = mux.read(i) * 10 / 1023 + 1;
    else {
      data[i] = mux.read(i);
      data[i] = (data[i] * 75 / 1023 + 5);
      data[i] = map(data[i], 5, 80, 80, 5);
      data[i] = data[i] / 100;
    }
  }
  */
}
MUX74HC4067 mux(7, 8, 9, 10, 11);
unsigned long currentTime = 0, previousTime = 0, deltaTime = 0; // if co 100ms
double data[10];  // zmienna do przechowywania danych z potencjometrów
double d1 = 0, d2 = 0, d3 = 0, d4 = 0;  // przepływ wody
Heater h1, h2, h3, h4;  // grzałki
Tank t1, t2, t3, t4;  // zbiorniki
Valve v1, v2, v3, v4, v5, v6;  // zawory
void setup() {
  Serial.begin(9600);
  mux.signalPin(A0, INPUT, ANALOG);
  cmdMessenger.attach(OnUnknownCommand);
  cmdMessenger.attach(kSend, OnReceive);
}
void loop() {

  Wire.requestFrom(8, 14);
  while (Wire.available()) {
    for (int i = 0; i < sizeof(x); i++) {
      x[i] = Wire.read();
    }
  }

  (x[0] == 1) ? v1.status = 1 : v1.status = 0;
  (x[1] == 1) ? v2.status = 1 : v2.status = 0;
  (x[2] == 1) ? v3.status = 1 : v3.status = 0;
  (x[3] == 1) ? v4.status = 1 : v4.status = 0;
  (x[4] == 1) ? v5.status = 1 : v5.status = 0;
  (x[5] == 1) ? v6.status = 1 : v6.status = 0;
  (x[6] == 1) ? h1.status = 1 : h1.status = 0;
  (x[7] == 1) ? h2.status = 1 : h2.status = 0;
  (x[8] == 1) ? h3.status = 1 : h3.status = 0;
  (x[9] == 1) ? h4.status = 1 : h4.status = 0;
  (x[10] == 1) ? t1.m = 1 : t1.m = 0;
  (x[11] == 1) ? t2.m = 1 : t2.m = 0;
  (x[12] == 1) ? t3.m = 1 : t3.m = 0;
  (x[13] == 1) ? t4.m = 1 : t4.m = 0;

  currentTime = millis();
  if (currentTime - previousTime >= 100UL) {
    muxRead();
    v1.qV(data[0]);
    v2.qV(data[1]);
    v3.qV(data[2]);
    v4.qV(data[3]);
    v5.qV(data[4]);
    v6.qV(data[5]);
    h1.dH(data[6]);
    h2.dH(data[7]);
    h3.dH(data[8]);
    h4.dH(data[9]);
    if (v1.status == 1) d1 += bind(t1, v1, 1);
    if (v2.status == 1) d1 += bind(t1, v2, -1, t1.hW);
    if (v2.status == 1) d2 += bind(t2, v2, 1, t1.hW);
    if (h2.status == 1) mixTemp(t2, t1, d2);
    t1.hW += d1; d1 = 0;
    if (v3.status == 1) d1 += bind(t1, v3, -1, t1.hW);
    if (v4.status == 1) d2 += bind(t2, v4, -1, t2.hW);
    if (v3.status == 1) d3 += bind(t3, v3, 1, t1.hW);
    if (h3.status == 1) mixTemp(t3, t1, d3);
    if (v5.status == 1) d3 += bind(t3, v5, -1, t3.hW);
    if (v4.status == 1) d4 += bind(t4, v4, 1, t2.hW);
    if (h4.status == 1) mixTemp(t4, t2, d4);
    t4.hW += d4; d4 = 0;
    if (v5.status == 1) d4 += bind(t4, v5, 1, t3.hW);
    if (h4.status == 1) mixTemp(t4, t3, d4);
    if (v6.status == 1) d4 += bind(t4, v6, -1, t4.hW);
    t1.hW += d1; t2.hW += d2; t3.hW += d3; t4.hW += d4;
    t1.check(); t2.check(); t3.check(); t4.check();
    if (h1.status == 1) heat(t1, h1);
    else cool(t1, h1);
    if (h2.status == 1) heat(t2, h2);
    else cool(t2, h2);
    if (h3.status == 1) heat(t3, h3);
    else cool(t3, h3);
    if (h4.status == 1) heat(t4, h4);
    else cool(t4, h4);
    d1 = 0; d2 = 0; d3 = 0; d4 = 0;
    previousTime = currentTime;
  }
  Wire.beginTransmission(8);
  y[0] = t1.x1;
  y[1] = t1.x2;
  y[2] = t1.x3;

  y[3] = t2.x1;
  y[4] = t2.x2;
  y[5] = t2.x3;

  y[6] = t3.x1;
  y[7] = t3.x2;
  y[8] = t3.x3;

  y[9] = t4.x1;
  y[10] = t4.x2;
  y[11] = t4.x3;

  y[12] = t1.xT;
  y[13] = t2.xT;
  y[14] = t3.xT;
  y[15] = t4.xT;

  Wire.write(y,16);
  Wire.endTransmission();

  cmdMessenger.feedinSerialData();
  OnSend();
}


void OnSend(){
  String message = "." + String(v1.status) + ":" + String(v2.status) + ":" + String(v3.status) + ":" + String(v4.status) + ":" + String(v5.status) + ":" + String(v6.status) + ":" + String(t1.m) + ":" + String(t2.m) + ":" + String(t3.m) + ":" + String(t4.m) + ":" +
  String(t1.hW) + ":" + String(t1.tWp) + String(t2.hW) + ":" + String(t2.tWp) + String(t3.hW) + ":" + String(t3.tWp) + String(t4.hW) + ":" + String(t4.tWp);
  cmdMessenger.sendCmd(kSend, message);
}

void OnUnknownCommand()
{
  cmdMessenger.sendCmd(kError,"Command without attached callback");
}

void OnReceive(){
  const char *str = cmdMessenger.readStringArg();
  sscanf(str, "%d:%d:%d:%d:%d:%d:%d:%d:%d:%d", &data[0], &data[1], &data[2], &data[3], &data[4], &data[5], &data[6], &data[7], &data[8], &data[9]);
}