#include <LiquidCrystal_I2C.h>
#include <Arduino.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include <Servo.h>
#include <TM1637.h>


LiquidCrystal_I2C lcd(0x27,20,4);

//servo pins
Servo servo_3;
Servo servo_6;

//segment screen
const int CLK = 10;
const int DIO = 11;
TM1637 tm(CLK, DIO);

///LOCAL OR GLOBAL?////GENERIC SELECTOR
//int NEXT_NEW;
//int PREV_NEW;
boolean NEXT_OLD;
boolean PREV_OLD;
//int VARIABLE_TO_CHANGE = 0;
/////////////////////////////////////
int PROGRAM_X = 0;
/////////////////////////////////
int BUTTON_NEW = 0;
int BUTTON_OLD = 0;
int SWITCH_STATE = 0;
int LCD_REFRESH = 0;
//////////////////////////////
int NEXT_PROGRAM_NEW = 0;
int NEXT_PROGRAM_OLD = 0;
int PREV_PROGRAM_NEW = 0;
int PREV_PROGRAM_OLD = 0;
////////////////////////////////
int PWM_CALIB_1 ;
int PWM_CALIB_2 ;
//VARIABLES FOR RPM
int RPM1 = 2000;
int RPM2 = 2000;

/*
// Coefficients fitting with 3th order polynom the settings of the controllers
// Line order: Front brush (RPM to muS), Back brush(RPM to muS), Fan (W to PWM), Pump (cc/min to PWM)
const float Coefficients [2][4] = {{ 9.98596E-09, -0.000156827, 0.889816665, -129.2423464},
                                   { 1.05263E-08, -0.000163382, 0.915024043, -160.126204}};
*/

void ON_OFF (){

  BUTTON_NEW = digitalRead(9);

  if(!((BUTTON_NEW == BUTTON_OLD))){
      delay(50);
  }
  BUTTON_NEW = digitalRead(9);
  if((!((BUTTON_NEW == BUTTON_OLD)))  &&  (BUTTON_NEW == 1)){
      if(SWITCH_STATE == 0){
          SWITCH_STATE = 1;

      }

      else{
          SWITCH_STATE = 0;
      }

      LCD_REFRESH = 0;

  }
  BUTTON_OLD = BUTTON_NEW;

}



int GENERIC_SELECTOR (int VARIABLE_TO_CHANGE){


    boolean NEXT_NEW = digitalRead(7);
    boolean PREV_NEW = digitalRead(8);


    if(NEXT_NEW == NEXT_OLD){
        delay(20);
        NEXT_NEW = digitalRead(7);
        
        if((NEXT_NEW == NEXT_OLD)  &&  (NEXT_NEW == 1)){
            if(PWM_CALIB_1 < 1900){
            VARIABLE_TO_CHANGE = (VARIABLE_TO_CHANGE + 100);
            }

        LCD_REFRESH = 0;
        }
         
    }

    if(PREV_NEW == PREV_OLD){
        delay(20);
        PREV_NEW = digitalRead(8);

        if((PREV_NEW == PREV_OLD)  &&  (PREV_NEW == 1)){
            if(PWM_CALIB_1 > 1150){
            VARIABLE_TO_CHANGE = (VARIABLE_TO_CHANGE - 100);
            }
            LCD_REFRESH = 0;
        }

    }
    
    NEXT_OLD = NEXT_NEW;
    PREV_OLD = PREV_NEW;
     
    return VARIABLE_TO_CHANGE;

}


void CALIBRATION_M1 (){

    

  if((SWITCH_STATE == 1)  &&  (LCD_REFRESH == 0)){
      //analogWrite(3,PWM_CALIB_1);
      servo_3.writeMicroseconds(PWM_CALIB_1);
      servo_6.writeMicroseconds(PWM_CALIB_1);

      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("CALIBRATION M1: ") + String(" ON"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM: ") + String(PWM_CALIB_1));
      lcd.setCursor(0,2);
      lcd.print(String("RPM: ") + String(RPM1));
      
      
      //show RPM in segment screen
      
      tm.display(0, (RPM1 / 1000) % 10);
      tm.display(1, (RPM1 / 100) % 10);
      tm.display(2, (RPM1 / 10) % 10);
      tm.display(3, RPM1 % 10);
      

      //show pwm in segment screen
      /*
      tm.display(0, (PWM_CALIB_1 / 1000) % 10);
      tm.display(1, (PWM_CALIB_1 / 100) % 10);
      tm.display(2, (PWM_CALIB_1 / 10) % 10);
      tm.display(3, PWM_CALIB_1 % 10);
      */

      LCD_REFRESH = 1;

  }
  
  if((SWITCH_STATE == 0)  &&  (LCD_REFRESH == 0)){
      
      servo_3.writeMicroseconds(1100);
      servo_6.writeMicroseconds(1100);
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(String("CALIBRATION M1: ") + String("OFF"));
      lcd.setCursor(0,1);
      lcd.print(String("PWM: ") + String(PWM_CALIB_1));
      lcd.setCursor(0,2);
      lcd.print(String("RPM: ") + String(RPM1));
      
      tm.display(0, 0);
      tm.display(1, 0);
      tm.display(2, 0);
      tm.display(3, 0);
      
      LCD_REFRESH = 1;

  }

}

/*int Set_Controller(int Setting, byte motor)
// Convert RPM, Power and Flowrate into controller settings with 3rd order polynom
// Motor: Front Brush == 0; Back Brush == 1; Fan == 2; Pump == 3
{
  int Val = (Coefficients[motor][0] * pow(Setting,3)) + (Coefficients[motor][1] * pow(Setting,2)) + 
     (Coefficients[motor][2] * Setting) + Coefficients[motor][3];
     if (motor == 2 && Setting <= 10) Val = Setting * 4;  //Work around for low fan powers drying cycle
  return Val;
}*/



void setup() {

  //initialize lcd
  lcd.init();
  lcd.backlight();

  //initialize segment display
  tm.init();
  tm.set(BRIGHT_TYPICAL);

  //buttons
  pinMode(7,INPUT);
  pinMode(8,INPUT);
  pinMode(9,INPUT);

  //segment screen
  pinMode(11,OUTPUT);
  pinMode(10,OUTPUT);
 
  //servo pins
  servo_3.attach(3);
  servo_6.attach(6);

}


void loop() {
      
      ON_OFF();

      //PWM_CALIB_1 = GENERIC_SELECTOR(PWM_CALIB_1);
      RPM1 = GENERIC_SELECTOR(RPM1);
      RPM2 = RPM1;
     
      //CALIBRATION CURVE FOR THE MOTOR 1 equation from excell file
      PWM_CALIB_1 = (9.71234E-09 * pow(RPM1,3)) + (-0.000152735 * pow(RPM1,2)) + (0.868887951 * RPM1) + (-88.76668555) ;
      //CALIBRATION CURVE FOR THE MOTOR 2 equation from excell file
      PWM_CALIB_2 = (1.24133E-08 * pow(RPM1,3)) + (-0.00018349 * pow(RPM1,2)) + (0.989405651 * RPM1) + (-232.6645872) ;

      CALIBRATION_M1 ();   

}