#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[24] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
byte doWizualizacji[23] = { 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 };
byte odWizualizacji[10] = { 10,10,10,10,10,10,5,5,5,5 };

double data[10];


int checkIfOk = true;

//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;
        checkIfOk = 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) {
        checkIfOk = 0;
        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 {
    h.check();
    t.tWn += 2 ;
  }
  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;
}
unsigned long currentTime = 0, previousTime = 0, deltaTime = 0; // if co 100ms
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() {
  Wire.begin();
  Serial.begin(9600);
  //cmdMessenger.attach(OnUnknownCommand);
  //cmdMessenger.attach(kSend, OnReceive);
  //Serial.print("odbuta");
}
void loop() {

  Wire.requestFrom(8, 14);
  //Serial.print("REGULATOR : ");
  while (Wire.available()) {
    for (int i = 0; i < sizeof(x); i++) {
      x[i] = Wire.read();
      //Serial.print(x[i]);
    }
  }
  //Serial.println();
  //Serial.print("POZIOM : ");
  //Serial.println(t1.hW);
  currentTime = millis();
  if (currentTime - previousTime >= 100UL) {

  (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;

   /*  v1.qV(1);
    v2.qV(11);
    v3.qV(11);
    v4.qV(11);
    v5.qV(11);
    v6.qV(11);
    h1.dH(0.8);
    h2.dH(0.8);
    h3.dH(0.8);
    h4.dH(0.8);*/
    v1.qV(odWizualizacji[0]);
    v2.qV(odWizualizacji[1]);
    v3.qV(odWizualizacji[2]);
    v4.qV(odWizualizacji[3]);
    v5.qV(odWizualizacji[4]);
    v6.qV(odWizualizacji[5]);
    h1.dH(odWizualizacji[6]/100);
    h2.dH(odWizualizacji[7]/100);
    h3.dH(odWizualizacji[8]/100);
    h4.dH(odWizualizacji[9]/100);
    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.requestFrom(9, 10);
   Serial.print("WIZUALIZACJA : ");
    while (Wire.available()) {
    for (int i = 0; i < sizeof(odWizualizacji); i++) {
         odWizualizacji[i] = Wire.read();
         if(odWizualizacji[i] == 0 && i < 6)
          odWizualizacji[i] = 11;
          if(odWizualizacji[i] == 0 && i > 6)
          odWizualizacji[i] = 5;
      Serial.print(odWizualizacji[i]);
      Serial.print(" | ");
    }
  }
  Serial.println();





  Wire.beginTransmission(9);
  Wire.write(doWizualizacji,23);
  Wire.endTransmission();

  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;

  x[16] = checkIfOk;

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

  //Serial.print("XD");
  //cmdMessenger.feedinSerialData();
  //OnSend();

doWizualizacji[0] = v1.status;
doWizualizacji[1] = v2.status;
doWizualizacji[2] = v3.status;
doWizualizacji[3] = v4.status;
doWizualizacji[4] = v5.status;
doWizualizacji[5] = v6.status;

doWizualizacji[6] = t1.m;
doWizualizacji[7] = t2.m;
doWizualizacji[8] = t3.m;
doWizualizacji[9] = t4.m;

doWizualizacji[10] = h1.status;
doWizualizacji[11] = h2.status;
doWizualizacji[12] = h3.status;
doWizualizacji[13] = h4.status;

doWizualizacji[14] = t1.hW * 100;
doWizualizacji[15] = t1.tWp;

doWizualizacji[16] = t2.hW * 100;
doWizualizacji[17] = t2.tWp;

doWizualizacji[18] = t3.hW * 100;
doWizualizacji[19] = t3.tWp;

doWizualizacji[20] = t4.hW * 100;
doWizualizacji[21] = t4.tWp;

doWizualizacji[22] = checkIfOk;
}