#include "GyverStepper2.h"
#include <Wire.h>               // Подключаем библиотеку Wire 
#include <Adafruit_INA219.h>    // Подключаем библиотеку Adafruit_INA219
#include "GyverTimers.h"
#include <SoftwareSerial.h>
#include "GyverPID.h"

#define PASSWORD 180491

#define MES_TIME  20

#define RREF 430.0 
#define RNOMINAL  100.0 

#define DEVICE0   0
#define DEVICE1   1

#define PAGE0     0
#define PAGE1     1

#define ON        1
#define OFF       0

#define CH1       5
#define CH2       6

#define OK        1


#define TP        0
#define TA        1
#define T1        2

#define P         3
#define I         4
#define D         5
#define PWM       6

#define TEST     10

#define CNT_PARAM 7

#define FLOAT     1
#define INT       0

#define NO_ENTER  1

#define BTN_LOAD_DISP 2

#define START    255

struct pid_st
{
  uint8_t switch_;
  float dest_temp1;
  float dest_temp2;
  float period;
  float pwm_time;
  uint8_t current_page;
  float pwm;
  long timer_time_pwm;
  long timer_time_mes;
  uint8_t param[CNT_PARAM];
  uint8_t cnt_num;
  uint8_t float_;

  float p;
  float i;
  float d;

  float * current_mes;
  float div;

  float service_pass;

  int pass_ok;
};

#define STOP          0
#define STOP_BUTTON   A1

uint16_t direction = 0;

#define STEP 5
#define DIR  6

GStepper2<STEPPER2WIRE>  stepper1(400, STEP, DIR, 7);
Adafruit_INA219 ina219;         // Создаем объект ina219

SoftwareSerial mySerial(8, 3); // RX, TX


GyverPID regulator(0.10, 0.08, 0.001, 100); 

pid_st pid;


/////////LCD
void init_lcd()
{
    mySerial.begin(9600);
}

void init_page0(pid_st * pid)///инициализация начальных значений страницы 0
{
  if(pid->switch_ == OFF)
  {
    mySerial.print("b1.bco=0");
    print_end();
  }
  else
  {
    mySerial.print("b1.bco=1024");
    print_end();
  }

  mySerial.print("t2.txt=\""+ String(int(pid->period)) +"\"");
  print_end();  

  pid->pass_ok = 0;
  pid->service_pass = 0;
}

void init_page1(pid_st * pid)///инициализация начальных значений страницы 1
{


  mySerial.print("t1.txt=\"");
  mySerial.print(pid->dest_temp1,1);
  mySerial.print("\"");
  print_end();  

  mySerial.print("t3.txt=\"");
  mySerial.print(pid->dest_temp2,1);
  mySerial.print("\"");
  print_end();  

  mySerial.print("t5.txt=\"");
  mySerial.print(int(pid->period));
  mySerial.print("\"");
  print_end();  
}

void init_page2(pid_st * pid)///инициализация начальных значений страницы 2
{
  mySerial.print("t3.txt=\"" + String(pid->p) +"\"");
  print_end();  

  mySerial.print("t4.txt=\""+ String(pid->i) +"\"");
  print_end();  

  mySerial.print("t5.txt=\""+ String(pid->d) +"\"");
  print_end();  

  mySerial.print("t7.txt=\""+ String(pid->pwm_time) +"\"");
  print_end();  

}


void signal_alarm(pid_st * pid)///установка на дисплее значение срабатывание датчика аварии
{
  if(pid->current_page == PAGE0)
  {
    mySerial.print("b1.bco=63488");
    print_end();
    mySerial.print("t1.bco=63488");
    print_end();
  }
}

void get_keyboard(pid_st * pid, String str)
{
  int begin = str.indexOf("key=");
  char num = str[begin + strlen("key=")];
  Serial.println(num);
  if((num == 0x2E) or (num == 0x2C))
  {
    pid->float_ = 1;
    pid->cnt_num = 1;
    return 0;
  }

  if(test_param(pid) == NO_ENTER)
  {
    pid->current_mes = &pid->service_pass;
    Serial.println("no enter");
    pid->div = 0;
  }

  if(pid->float_ == INT)
  {
    if(pid->cnt_num > 0)
    {
      *pid->current_mes = (*pid->current_mes * 10.0 ) + float(num-0x30);
    }
    else
    {
      *pid->current_mes = float(num-0x30);
    }   
  }
  else
  {
    float mn = 1;
    for(int i = 0; i < pid->cnt_num; i++)
    {
      mn *= 10;
    }
    pid->div +=  float(num-0x30) / mn;

    *pid->current_mes = float(int(*pid->current_mes)) + pid->div;
  }

  pid->cnt_num++;
  Serial.println("CNt_num = " + String(pid->cnt_num));

  if(pid->service_pass == PASSWORD)
  {
    pid->service_pass = 0;
    go_page(2);
    pid->pass_ok = OK;

  }

  if(pid->pass_ok == OK)
  {
    init_page2(pid);
  }
  else
  {
    init_page1(pid);
  }


  
}

uint8_t test_param(pid_st * pid)
{
  uint8_t param_sum = 0;

  for(int i = 0; i < CNT_PARAM; i++)
  {
    param_sum +=pid->param[i];
  }

  if(param_sum == 0)
  {
    return OK;
  }
  
  return 0;
}


void print_end()///обязательная отправка в конце каждой посылки на дисплей
{
    mySerial.write(0xff);
    mySerial.write(0xff);
    mySerial.write(0xff);
}



void page_next(pid_st * pid)///переход на следующую страницу
{
  if(pid->current_page == PAGE0)
  {
    go_page(PAGE1);
    pid->current_page = PAGE1;
 
    init_page1(pid);
  }
  else
  {
    go_page(PAGE0);
    pid->current_page = PAGE0;  
    //pid->switch_ = OFF;  

    init_page0(pid);
  }
}

void go_page(uint8_t page)///переход страницу
{
    mySerial.print("page page" + String(page));
    print_end();
}


void init_pin()
{
  pinMode(STOP_BUTTON, INPUT);
  digitalWrite(BTN_LOAD_DISP, HIGH);
}


/*
void test_stop_motor()
{
  if((digitalRead(STOP_BUTTON) == STOP) and (direction < 0))
  {
    stepper1.brake();
    Serial.println("motor close go");
  }
}*/

volatile int cur_pos = 400;
volatile int step = 0;
ISR(TIMER1_A) {  // пишем  в сериал
  step = calc_pid(40000, emulate(cur_pos));

  set_position(step);
}

void set_position(int pos)
{
  stepper1.setTarget(pos);
}

uint32_t emulate(int pos)
{
  static unsigned long time_start = 0;
  static uint32_t mes            = 0;
  static uint32_t output = 0;
  static uint32_t cur_press = 65000;


    time_start = millis();
    mes++;
    output += (900.0 *  (step / 255.0));
    ///output = 65000 - (mes * (1000.0 * (pos/400.0)));
    Serial.println("PRESS = " + String(output));
    Serial.println("STEP = " + String(step));
    Serial.println("POS = " + String(cur_pos));

  return output;
}

void init_pid()///инициализация начальных значений ПИД
{
  regulator.setDirection(NORMAL); // направление регулирования (NORMAL/REVERSE). ПО УМОЛЧАНИЮ СТОИТ NORMAL
  regulator.setLimits(0, 255);   // пределы . ПО УМОЛЧАНИЮ СТОЯТ 0 И 255
  regulator.setMode(ON_RATE);
}

int calc_pid(uint32_t press, float cur_press)
{
  volatile uint32_t step = press / 60;
  ///Serial.println("STEP_PID = " + String(step));
  volatile static float press_per_time = 0;

  press_per_time+=step;
  
  /*if(press_per_time < press * 0.5)
  {
    regulator.setpoint = press_per_time;
  }
  else
  {
    regulator.setpoint = press;
  }*/

  /*if(press_per_time < cur_press)
  {
    regulator.setDirection(REVERSE);
  }
  else
  {
    regulator.setDirection(NORMAL);
  }*/

  regulator.setpoint = press;
  Serial.println("PRESSperTime = " + String(press_per_time));
  Serial.println("cur_press = " + String(cur_press));
  regulator.input = cur_press;



  return regulator.getResultTimer();
}

void setup() {
  stepper1.enable();
  stepper1.setSpeed(800);
  stepper1.reverse(1);
  
  Serial.begin(9600); 
  mySerial.begin(9600);
  uint32_t currentFrequency;    // Создаем переменную
    
// По умолчанию при инициализации будет использоваться самый большой диапазон (32 В, 2 А)
// Проверка инициализации модуля INA219

  init_pid();

  if (! ina219.begin()) {       
    Serial.println("Failed to find INA219 chip");  
    ///while (1) { delay(10); }
  }

// Чтобы использовать немного более низкий диапазон 32 В, 1 А (более высокая точность на усилителях):
// ina219.setCalibration_32V_1A ();
// Или использовать более низкий диапазон 16 В, 400 мА (более высокая точность на вольт и ампер):
// ina219.setCalibration_16V_400mA ();
   
  Serial.println("Measuring voltage and current with INA219 ..."); // Отправка сообщение

  //go_page(1);
  //pid.current_page = PAGE1;
  ///init_page0(&pid);

  //set_position(START);

  unsigned long start = 0;
  /*while(1)
  {
    stepper1.tick();
    if(millis() - start > 4000)
    {
      break;
    }
  }*/
  Timer1.setFrequencyFloat(5); 
  Timer1.enableISR();
}

int dir = 1;
void loop() {
  ///load_disp();
  stepper1.tick();
  ///test_stop_motor();

  /*if(Serial.available())
  {
    char cmd = Serial.read();
    if(cmd == 'c')
    {
      direction = 0;
      Serial.println("motor close go");
      stepper1.setTarget(direction);
    }
    if(cmd == 'o')
    {
      direction = -400;
      Serial.println("motor open go");
      stepper1.setTarget(direction);
    }
    //stepper1.reverse(true);
  }*/

  /*Serial.println(analogRead(A3) * (5.0/1024.0));
  delay(100);*/
  ///stepper1.tick();
  /*stepper1.step();  
   if(Serial.available())
  {
    Serial.read();
    stepper1.reverse(true);
  }*/
  /*
  stepper1.setTarget(-2000);


  unsigned long time_exit = millis();
  while(1)
  {
    if(stepper1.ready())
    {
      break;
    }
    float current_mA = ina219.getCurrent_mA(); 
    Serial.print("Current:       "); Serial.print(current_mA); Serial.println(" mA");

    if(current_mA > 100)
    {
      stepper1.stop();
      break;
    }
  }

  stepper1.setTarget(400);

time_exit = millis();
  while(1)
  {
    if(stepper1.ready())
    {
      break;
    }
    float current_mA = ina219.getCurrent_mA(); 
    Serial.print("Current:       "); Serial.print(current_mA); Serial.println(" mA");

    if(current_mA > 1000)
    {
      stepper1.stop();
      break;
    }
  }*/

}