#include "MUX74HC4067.h"

#define WYP  0.63 // współczynnik wypływu dla ostrych brzegów
//#define PI  3.14
#define G 9.81
#define TM 6 // 100ms/1 cykl symulacji = 6 sekund dla rzeczywistego obiektu
#define QM 11 // maks wartosć potencjometrów
#define X1 0.6
#define X2 1.3
#define X3 1.9


// Creates a MUX74HC4067 instance
// 1st argument is the Arduino PIN to which the EN pin connects
// 2nd-5th arguments are the Arduino PINs to which the S0-S3 pins connect
MUX74HC4067 mux(7, 8, 9, 10, 11);

unsigned long currentTime = 0;
unsigned long previousTime = 0;
unsigned long deltaTime = 0;
double data[10];

/* --- Przyciski --- */
int button1Pin = 2, button2Pin = 3, button3Pin =4, button4Pin = 5, button5Pin = 6,  button6Pin = 7;
int button1State = LOW, button2State = LOW, button3State = LOW, button4State = LOW, button5State = LOW, button6State = LOW;
int button1Previous = LOW, button2Previous = LOW, button3Previous = LOW, button4Previous = LOW, button5Previous = LOW, button6Previous = LOW;
unsigned long time1 = 0, time2 = 0, time3 = 0, time4 = 0, time5 = 0, time6 = 0;
long debounce = 200;
/* --- Przyciski --- */

    double d1 = 0, d2 = 0, d3 = 0, d4 = 0; // różnica w poziomie wodym w całym cyklu
    class Tank // zmiany tu
    {
      public:
      int x1 = 0, x2 = 0, x3 = 0;
      double hT = 2; // wysokość zbiornika w m2
      double r = 0.5; // promień zbiornika w m
      double sT = PI * r * r; // powierzchnia podstawy zbiornika w m2
      double vT = sT * hT; // objętość zbiornika w m3
      double hW = 0.0; // poziom wody w zbiorniku w m
      double vW = 0; // objętość wody w zbiorniku w m3 50litra max objętość
      double tWp = 20;
      double tWn = 20;
      int m = 0; // mieszadło on/off
      int overflowed = 0; // czujnik przelania
      void check();

    };
     void Tank::check()
     {
      if (hW>=hT)
      {
        hW=2;
        overflowed = 1;
      }
      else overflowed = 0;
      (hW>=X1)?x1 = 1:x1 = 0;
      (hW>=X2)?x2 = 1:x2 = 0;
      (hW>=X3)?x3 = 1:x3 = 0;
      vW=sT*hW;
     }

    class Valve
    {
      public:
      int status = 0; // 1 nalewa/ 0 nic nie robi
      double r = 0.03; // promień przekorju zaworu w m
      double d = 1; // procent otwarcia zaworu
      double sV = PI * r * r; // przekrój otworu w m2
      double rSV = sV; // Obecny przekrój
      void qV(double x)
      {
        if (x != QM)
        {
        rSV = sV / x;
        }
        else
        {
          rSV = 0;
        }
      }
    };

    class Heater // zmiany tu
    {
      public:
      int status = 0; // 1 grzeje/ o nie grzeje
      int p=100;
      double alfa1=2.8;
      double alfa2=-0.8;
      double alfa3=0.04;
      double tHp = 20;
      double tHn = 20;

      void dH(double x)
      {
          alfa2=-x;
      }
    };

    void heat(Tank& t, Heater& h) // zmiany tu
    {
      h.tHn = h.tHp + 0.1 * (h.p-h.alfa1*(h.tHp - t.tWp));
      if(t.vW)
      t.tWn = t.tWp + 0.1 * (h.alfa2 * 1/t.vW * (t.tWp - h.tHp) + h.alfa3*(t.tWp - 20));
      else t.tWn = 20;
      if(t.tWn > 100)
      {
        t.tWn = 100;
      }
      t.tWp = t.tWn;
      h.tHp = h.tHn;

    }

    void cool(Tank& t, Heater& h) // zmiany tu
    {
      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) // zmiany tu
    {
      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); // natężenie wypływu w m3/s

        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() // zmiany tu
    {

      for (int i = 0; i < 10; ++i)
      {
      // Reads from channel i. Returns a value from 0 to 1023
      //data[i] = mux.read(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;
      }
      }
    }

        //int i=0;

        Heater h1,h2,h3,h4;
        Tank t1,t2,t3,t4;
        Valve v1,v2,v3,v4,v5,v6;

    void setup() {
      // put your setup code here, to run once:
      Serial.begin(9600);
        pinMode(button1Pin, INPUT);
        pinMode(button2Pin, INPUT);
        pinMode(button3Pin, INPUT);
        pinMode(button4Pin, INPUT);

  // Configures how the SIG pin will be interfaced
  // e.g. The SIG pin connects to PIN A0 on the Arduino,
      //      and PIN A0 is a analog input
       mux.signalPin(A0, INPUT, ANALOG);

    }

    void loop() {

    currentTime = millis();
    if (currentTime - previousTime >= 100UL) // co 100ms
    {
           { // obsługa guzików
      button1State = digitalRead(button1Pin);
      if (button1State == HIGH && button1Previous == LOW && millis() - time1 > debounce)
      {

        if (v1.status == HIGH)
        {
          v1.status = LOW;
        }
        else
          v1.status = HIGH;

        time1 = millis();
      }
      button1Previous = button1State;

     /* button2State = digitalRead(button2Pin);
      if (button2State == HIGH && button2Previous == LOW && millis() - time2 > debounce)
      {
        if (v2.status == HIGH)
          v2.status = LOW;
        else
          v2.status = HIGH;

        time2 = millis();
      }
      button2Previous = button2State;

      button3State = digitalRead(button3Pin);
      if (button3State == HIGH && button3Previous == LOW && millis() - time3 > debounce)
      {
        if (v3.status == HIGH)
          v3.status = LOW;
        else
          v3.status = HIGH;
        time3 = millis();
      }
      button3Previous = button3State;


      button4State = digitalRead(button4Pin);
      if (button4State == HIGH && button4Previous == LOW && millis() - time4 > debounce)
      {
        if (v4.status == HIGH)
          v4.status = LOW;
        else
          v4.status = HIGH;
        time4 = millis();
      }
      button4Previous = button4State;


      button5State = digitalRead(button5Pin);
      if (button5State == HIGH && button5Previous == LOW && millis() - time5 > debounce)
      {
        if (v5.status == HIGH)
          v5.status = LOW;
        else
          v5.status = HIGH;
        time5 = millis();
      }
      button5Previous = button5State;


      button6State = digitalRead(button6Pin);
      if (button6State == HIGH && button6Previous == LOW && millis() - time6 > debounce)
      {
        if (v6.status == HIGH)
          v6.status = LOW;
        else
          v6.status = HIGH;
        time6 = millis();
      }
      button6Previous = button6State;
      */
    } // koniec obsługi guzikówm

        muxRead();// zmiany tu
        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]);

        // zmiany tu
        if (v1.status == HIGH) d1 += bind(t1, v1, 1);
        if (v2.status == HIGH)  d1 += bind(t1, v2, -1, t1.hW);

        if (v2.status == HIGH) d2 += bind(t2, v2, 1, t1.hW);
        if (h2.status == 1)mixTemp(t2,t1,d2);
        t1.hW += d1;
        d1=0;
        if (v3.status == HIGH) d1 += bind(t1, v3, -1, t1.hW);

        if (v4.status == HIGH) d2 += bind(t2, v4, -1, t2.hW);

        if (v3.status == HIGH) d3 += bind(t3, v3, 1, t1.hW);
        if (h3.status == 1)mixTemp(t3,t1,d3);
        if (v5.status == HIGH) d3 += bind(t3, v5, -1, t3.hW);

        if (v4.status == HIGH) d4 += bind(t4, v4, 1, t2.hW);
        if (h4.status == 1)mixTemp(t4,t2,d4);
        t4.hW += d4;
        d4=0;
        if (v5.status == HIGH) d4 += bind(t4, v5, 1, t3.hW);
        if (h4.status == 1)mixTemp(t4,t3,d4);
        if (v6.status == HIGH) d4 += bind(t4, v6, -1, t4.hW);


        t1.hW += d1; // zmiany tu
        t2.hW += d2;
        t3.hW += d3;
        t4.hW += d4;

         t1.check();
         t2.check();
         t3.check();
         t4.check();
         if (t1.overflowed==1)
         {
          t1.hW=2;
          }
         if (t2.overflowed==1)
         {
          t2.hW=2;
         }

         if (h1.status == 1) heat(t1,h1); // zmiany tu
         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);
      // put your main code here, to run repeatedly:
      Serial.print(t1.tWp);
      Serial.print(" | ");
      Serial.print(h1.tHp);
      Serial.print(" | ");
      Serial.print(t1.hW,5);
      Serial.print(" | ");
      Serial.print(t2.hW,5);
      Serial.print(" | ");
      //Serial.print(t1.tW);
      Serial.print(" | ");
      Serial.print(data[6]);
      Serial.print(" | ");
      Serial.print(data[1]);
      Serial.print(" | ");
      Serial.print(v2.status);
      Serial.print(" | ");
      Serial.print(v5.status);
      Serial.print(" | ");
      Serial.print(t4.hW,6);
      Serial.print(" | ");
      //Serial.print(d1);
      //int dp=analogRead(A1);
      //Serial.println(dp);


      Serial.println(v6.status);
      //Serial.println(button1State);
      //Serial.print(" | ");
      //Serial.println(t1.sZb,9);
      d1 = 0; d2 = 0; d3 = 0; d4 = 0;

      previousTime = currentTime; // do loopa co 100ms
  } // co 100ms
    }