//#include <Arduino.h>
#include <math.h>
#include <U8g2lib.h>

U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);  //Andreas
U8G2_SH1106_128X64_NONAME_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);  //Leonhard

void TIM1_init();
void TIM2_init();
void LCD_printData();
double get_tempValue(int SensorValue);
void LCD_printGrid();

int Temperatur;
int TempSensor = 0;
float Voltage;
int Ausgangssignal = 0;
int status = 0;
int Stellsignal = 0;
int counterx=0;
int i=2;


//Variablen Regelung

//Eingangssignal
double Sollwert=70;
//Abtastzeit
double Ta=1;
//PIDT-Regler Zeitkonstanten (multiplikative Form)
double VR=2;   
double TN=80;  
double TV=1;    
double TR=TV/10.0; 
//PIDT-Regler Zeitkonstanten (additive Form)
double VR_Stern=VR*(1.0+(TV/TN));
double TN_Stern=TN*(1.0+(TV/TN));
double TV_Stern=TV/(1.0+(TV/TN));
double TI=TN_Stern/VR_Stern;
double TD=TV_Stern*VR_Stern;
//Eingangssignal Regler ek
double ek=0;
//Globale Variable für Zeitdiskrete Regelung
double yk1=0;
double yk1vorher=0;
double yk2=0;
double yk2vorher=0;
double yk3=0;
double yk3vorher=0;
double yk4=0;
double yk4vorher=0;
//Ausgangssignal Regler yk
double yk=0;

void setup() 
{
  pinMode(13, OUTPUT);
  pinMode(3, OUTPUT);
  digitalWrite(3, LOW);
  u8g2.begin(); 
  u8g2.clearBuffer();
  LCD_printGrid();
  cli();        //disable global interrupts
  TIM1_init();  //Timer 1 für Ta=100ms
  TIM2_init();  //Timer 2 für PWM
  sei();        //enable global interrupts
}

void loop() 
{
  LCD_printData();
}

ISR(TIMER1_COMPA_vect) 
{
  status=1;
  digitalWrite(13, !digitalRead(13));  // toggle LED with T=TA
  TempSensor=analogRead(A0);
    if(595<=TempSensor)
    {
      Temperatur=get_tempValue(TempSensor);
    }
  ek=Sollwert-Temperatur;              //Eingangssignal
  //PT1-Anteil:
  yk1vorher=yk1vorher*(TR/(Ta+TR));
  yk1=ek*(Ta/(Ta+TR))+yk1vorher;
  yk1vorher=yk1;
  //P-Anteil:
  yk2=yk1*VR_Stern;
  //I-Anteil:
  yk3=yk1*(Ta/TI)+yk3vorher;
  yk3vorher=yk3;
  //D-Anteil:
  yk4=yk1*(TD/Ta)-yk4vorher;
  yk4vorher=(TD/Ta)*yk1;
  //Addition P-, I-, und D-Anteil
  yk=yk2+yk3+yk4;
  //Begrenzung Ausgangssignal auf 100
  if(yk>=100)
  {
    yk=100;
  }

  //PWM Ausgangssignal
  Stellsignal=255*(yk/100);
  if(yk>=100)
  {
    Stellsignal=255;
  }
  if(yk<=0)
  {
    Stellsignal=0;
  }
  OCR2B = Stellsignal;          //PWM Duty Cycle  
  counterx++;
}

void TIM1_init() 
{
  TCNT1 = 0;   //Timer Counter 1
  TCCR1A = 0;  //Timer Counter Controll Register A
  TCCR1B = 0;  //Timer Counter Controll Register B
  OCR1A = 15625;  //Output Compare Register A -> Interrupt each second (1/16*10^6)*1024*15625 = 1s
  TCCR1B |= 0x5;  //Prescaler = 1024
  TCCR1B |= 0x8;  //CTC Bit -> TOP of Counter = OCR1A
  TIMSK1 |= 0x2;  //Tim1 Interrupt @ Output Compare Match A
}


void TIM2_init() 
{
  TCNT2 = 0;           //Timer Counter 2
  TCCR2A = 0;          //Timer Counter Controll Register A
  TCCR2B = 0;          //Timer Counter Controll Register B
  TCCR2A |= 0x23;      //Non Inverting Mode
  TCCR2B |= 0x7;       //Prescaler = 1024
  OCR2B = 0;           //PWM Duty Cycle
  DDRD |= (1 << PD3);  //Port Output
}

double get_tempValue(int SensorValue)
{
  double a=0.0231;
  double b=0.26;
  double c=5.22;
  double calculatedVoltage=5.0*(SensorValue/1023.0);
  return log((c-calculatedVoltage)/b)*(1/a);
}

void LCD_printData()
{
  if(status==1)
  { 
    if(i>123)
    {
      LCD_printGrid();
      i=2;
    }
    u8g2.setFontMode(1);
    u8g2.setFont(u8g2_font_6x10_tf);
    u8g2.setDrawColor(1);
    u8g2.drawBox(0, 0, 127, 9);
    u8g2.setDrawColor(2);
    u8g2.drawBox(0, 0, 127, 9);
    u8g2.setCursor(2,8);
    u8g2.print(F("Ti:"));
    u8g2.print(get_tempValue(TempSensor),0);    
    if(counterx%5==0)    //each 5seconds new TempPlot
    {
      if(get_tempValue(TempSensor)>-5&&get_tempValue(TempSensor)<100)
      {
        u8g2.drawPixel(i++, 63-((get_tempValue(TempSensor)/100.0))*55);
      }
    }
    u8g2.setCursor(42,8);
    u8g2.print(F("Ts:"));
    u8g2.print(Sollwert,0);    
    u8g2.setCursor(84,8);
    u8g2.print(F("St:"));
    u8g2.print(yk,0); 
    u8g2.sendBuffer();
    status=0;
  }
}

void LCD_printGrid()
{
  u8g2.clearBuffer();
  u8g2.setFontMode(1);
  u8g2.drawLine(1, 63, 127, 63);
  u8g2.drawLine(0, 10, 0, 63);
  for(int a=10; a<=100; a=a+10)
  {
    u8g2.drawLine(0, (63-(a/100.0)*55), 2, (63-(a/100.0)*55));
  }   
  for(int a=10; a<127; a=a+10)
  {
    u8g2.drawLine(a, 61, a, 63);
  }    
  u8g2.sendBuffer();
}