#include <Math.h>
#include <TimerOne.h>
#include <PID_v1.h>
#include <RunningMedian.h>
#include <SmoothThermistor.h>

#define DV 18
#define vyskaHladinyPin 19
#define motorFREQPin 20
#define zerroCrossingPin 21

#define topeniPin 30
#define motorSmerPin 33
#define motorACPin 23
#define cerpadloVypousteniPin 32
#define vodaHlavniPin 31
#define vodaPredpirkaPin 29
#define cerpadloSprchaPin 28
#define dverePin 25

#define FREQ 50 // frekvence site
#define MOTOR_PWR_CONSTANT 80    // 0-8000 us = 0 - 100%
#define PREVODOVY_POMER 0.0375

#define VODA_MIN 22300
#define VODA_MAX 26000

volatile unsigned int vodaIndex = (VODA_MAX - VODA_MIN) / 100;

volatile unsigned long vTimerTmp = 0;
volatile unsigned long vTimerD = 0;
volatile unsigned long vTimerLast = 0;
volatile unsigned long vTimer0 = 0;
volatile unsigned long vTimer1 = 0;

volatile byte motorState = B00000000;
volatile unsigned int motorSmer = 0;
volatile double motorRPM = 0;
volatile double motorPWR = 0;
volatile double setMotorRPM = 0;
volatile double setMotorRPMtmp = 0;

volatile unsigned int dTime = 0;
volatile unsigned long motorTime = 0;
volatile unsigned int motorPeriodTimer = 0;
volatile unsigned long motorLastRPMtime = 0;
volatile unsigned long motorLastFREQtime = 0;
volatile unsigned long motorRPMtmp = 0;

volatile float teplota = 99;

volatile unsigned int vyskaHladinyInt = 0;
volatile unsigned long vyskaHladinyLast = 0;

volatile double slowKp = 0.003, slowKi = 0.002, slowKd = 0.0001;
volatile double consKp = 0.05, consKi = 0.05, consKd = 0.0005; //0.05, 0.05, 0.0005
PID motorPID(&motorRPM, &motorPWR, &setMotorRPMtmp, consKp, consKi, consKd, DIRECT);

RunningMedian motorFREQsamples = RunningMedian(5);
RunningMedian vyskaHladinySamples = RunningMedian(1000);
RunningMedian teplotaSamples = RunningMedian(10);

SmoothThermistor smoothThermistor(A0);

void setup()
{
  pinMode(motorACPin, OUTPUT);
  pinMode(motorSmerPin, OUTPUT);
  pinMode(topeniPin, OUTPUT);
  pinMode(cerpadloVypousteniPin, OUTPUT);
  pinMode(cerpadloSprchaPin, OUTPUT);
  pinMode(vodaHlavniPin, OUTPUT);
  pinMode(vodaPredpirkaPin, OUTPUT);
  pinMode(dverePin, OUTPUT);
  pinMode(zerroCrossingPin, INPUT_PULLUP);
  pinMode(motorFREQPin, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(zerroCrossingPin), zerroCrossing, FALLING); // FALLING RISING
  attachInterrupt(digitalPinToInterrupt(motorFREQPin), motorFREQ, RISING); // FALLING RISING
  attachInterrupt(digitalPinToInterrupt(vyskaHladinyPin), vyskaHladiny, RISING); // FALLING RISING
  motorPID.SetMode(AUTOMATIC);
  motorPID.SetOutputLimits(0, 255);
  motorPID.SetSampleTime(100); // 200ms default
  Timer1.initialize(motorPeriodTimer);
  smoothThermistor.useAREF(false);
}

void loop() {

  // reset otacek motoru po zastaveni motoru
  if (motorLastFREQtime < micros() - 60000) {
    motorRPM = 0;
  }


  // Program ......


  // obsluha motoru

  // Tuneni rozbehu motoru
  
  if (setMotorRPM > 700) {
    if ( motorLastRPMtime + 100 < millis()) {
      motorLastRPMtime = millis();

      if (setMotorRPMtmp < 300) {
        setMotorRPMtmp = 300;
      }

      if (setMotorRPMtmp < setMotorRPM) {
        setMotorRPMtmp = setMotorRPMtmp + 10;
      }

    }

  } else if (setMotorRPM > 0) {

    if ( motorLastRPMtime + 100 < millis()) {
      motorLastRPMtime = millis();

      if (setMotorRPMtmp < 100) {
        setMotorRPMtmp = 100;
      }

      if (setMotorRPMtmp < setMotorRPM) {
        setMotorRPMtmp = setMotorRPMtmp + 50;
      }
    }

  } else {
    setMotorRPMtmp = setMotorRPM;
  }

  noInterrupts();
  if (setMotorRPM > 0) {
    double gap = abs(setMotorRPMtmp - motorRPM);
    if (setMotorRPMtmp > 700 && gap > 1000) {
      motorPID.SetTunings(slowKp, slowKi, slowKd);
    } else {
      motorPID.SetTunings(consKp, consKi, consKd);
    }
    motorPID.Compute();

  } else {
    motorRPM = 0;
    setMotorRPMtmp = 0;
    motorPWR = 0;
  }
  interrupts();

  // merime teplotu
  
  if (millis() % 500 == 0) {
    float tmp = smoothThermistor.temperature();
    teplotaSamples.add(tmp);
    teplota = abs(teplotaSamples.getMedian());
  }

  // Inkrementujeme Timery o 1s
  
  vTimerTmp = millis();
  vTimerD = vTimerTmp - vTimerLast;
  if (vTimerD >= 1000 ) {
    vTimerLast = vTimerTmp;
    vTimer0++;
    vTimer1++;
  }
}

void vyskaHladiny() {
  noInterrupts();

  double tmp = micros();
  vyskaHladinySamples.add(tmp - vyskaHladinyLast);
  vyskaHladinyInt = abs(vyskaHladinySamples.getMedian());
  vyskaHladinyLast = tmp;

  interrupts();
}

void motorFREQ() {

  noInterrupts();

  motorLastFREQtime = micros();
  dTime = motorLastFREQtime - motorTime;
  motorTime = motorTime + dTime;
  motorRPMtmp = ((1000000 / dTime) * 60) / 8;
  motorFREQsamples.add(motorRPMtmp);
  motorRPM = abs(motorFREQsamples.getMedian());
  if (motorRPM < 0) {
    motorRPM = 0;
  }

  interrupts();
}

void zerroCrossing() {

  noInterrupts();

  if (motorPWR == 0) {
    digitalWrite(motorACPin, LOW);
    motorState = B00000000;
  }
  else if (motorPWR == 255) {
    digitalWrite(motorACPin, HIGH);
    motorState = B00000000;
  } else {
    int pwr = ( 8000 - ((8000 / 255) * motorPWR)  );
    Timer1.attachInterrupt(motorTickPWR, pwr);
    motorState = B00000001;
  }

  interrupts();
}

void motorTickPWR ()
{
  noInterrupts();

  Timer1.stop();

  if (motorState == B00000001) {
    digitalWrite(motorACPin, HIGH);
    int i = 100;
    while (i--) {
      __asm__("nop\n\t");
    }
    digitalWrite(motorACPin, LOW);
  }
  interrupts();
}