#include "lin_regr.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include "TM1637.h"

#define TRIG_PIN A3
#define ECHO_PIN A2

#define POT_PIN A0

#define LED_PIN 7
#define TFT_DC 9
#define TFT_CS 10

#define CLK 2
#define DIO 3

#define n_obs 20

#define markSize 5

#define DISTANCE_MAX  400 // cm
#define DISTANCE_MIN  0 // cm

Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);
TM1637 tm(CLK, DIO);

LinearRegression lr = LinearRegression();
double t = 0.0;   //  Время в секундах
const int step = 100;  // Временной шаг в мс
double T = step * n_obs / 1000;
const float t_plus = 0.25;

double tt[n_obs];
double yy[n_obs] = {354.27042161, 350.01055179, 336.11890445, 304.31840256,
       310.60514146, 284.02899907, 286.08112697, 256.71754787,
       229.53274819, 225.98366929, 191.77662261, 202.71471575,
       175.81327557, 164.03447675, 157.34125985, 120.79582701,
       118.71769451, 101.97502078,  83.27412851};
double regr[n_obs];
double temp;

double t_min, t_max, y_min, y_max;


void setup() {

  pinMode(POT_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  tm.init();
  tm.set(BRIGHT_TYPICAL);

  Serial.begin(9600);
  tft.begin();

  tft.setCursor(26, 120);
  tft.setTextColor(ILI9341_RED);
  tft.setTextSize(3);
}

void display(int dist){
  tm.display(0, (dist / 100) % 10);
  tm.display(1, (dist / 10) % 10);
  tm.display(2, dist % 10);
}

int get_distance() {
  static int distance;
  uint16_t duration = 0;
  uint32_t interval = 0;

  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(5);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Read time of the trig and echo pins
  duration = pulseIn(ECHO_PIN, HIGH);
  // Calculates the distance
  distance = (duration / 2) / 29;

  return distance; // centimeters
}

void get_data() {
  lr.reset();
  for (int i=0; i<n_obs; i++) {
    tt[i] = t;

    // yy[i] = get_distance();

    lr.learn(t, yy[i]);
    delay(step);
    t += step / 1000.0;
  }
}

void set_bounds() {

  y_min = DISTANCE_MIN;
  y_max = DISTANCE_MAX;

  t_min = tt[0];
  t_max = tt[n_obs-1] + t_plus;

}

void draw_predict() {
  set_bounds();
  tft.fillScreen(ILI9341_BLACK);
  int x;
  for (int i=0; i<n_obs; i++) {
    x = 320 - (tt[i] - t_min) / (t_max - t_min) * 320;
    int y = 240 - (yy[i] - y_min) / (y_max - y_min) * 240;
    tft.fillRect(y, x, markSize, markSize, ILI9341_WHITE); // точки на дисплее 
  }

  int y_lr_1 = (lr.calculate(tt[0]) - y_min) / (y_max - y_min) * 240;
  int y_lr_2 = (lr.calculate(t_max) - y_min) / (y_max - y_min) * 240;


  Serial.print("Speed: ");
  Serial.print(-lr.k/T / 100 * 3.6);
  Serial.print(" km/h\t");

  int lim = analogRead(POT_PIN) / 1023.0 * DISTANCE_MAX;
  display(lim);
  
  Serial.print("Threshold: ");
  Serial.print(lim);
  Serial.print(" cm\n");

  Serial.print("mean: ");
  Serial.print(lr.mean());
  Serial.print("\tstd: ");
  Serial.println(lr.std());

  int lim_disp = 240 - lim / (y_max - y_min) * 240;

  tft.drawLine(lim_disp, 0, lim_disp, 320, ILI9341_WHITE);

  if ((lr.calculate(t_max) <= lim) && (lr.k < 0)) {
    tft.drawLine(y_lr_1, 0, y_lr_2, 320, ILI9341_RED); // линия тренда на дисплее
    Serial.print("BRAKING\n");
    digitalWrite(LED_PIN, HIGH);
  }
  else {
    tft.drawLine(y_lr_1, 0, y_lr_2, 320, ILI9341_YELLOW); // линия тренда на дисплее
    Serial.print("\n");
    digitalWrite(LED_PIN, LOW);
  }
}

void loop() {
  get_data();
  draw_predict();
}