#include <TFT_eSPI.h>
#include <SPI.h>       // this is needed for display
//#include <Wire.h>      // this is needed for FT6206
//#include <Adafruit_FT6206.h>

// The FT6206 uses hardware I2C (SCL/SDA)
//Adafruit_FT6206 ctp = Adafruit_FT6206();

// The display also uses hardware SPI, plus #9 & #10
#define TFT_CS 15
#define TFT_DC 2
#define TFT_MOSI 23
#define TFT_SCLK 18
//Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);



long runningAverage(int M, TFT_eSPI *_tft) {
  #define LM_SIZE 10
  static int LM[LM_SIZE];      // LastMeasurements
  static byte index = 0;
  static long sum = 0;
  static byte count = 0;

  _tft->print("RND:");
  _tft->print(M);
  _tft->print(" INDEX:");
  _tft->print(index);
  _tft->print(" SUM:");
  _tft->print(sum);
  _tft->print(" COUNT:");
  _tft->print(count);

  // keep sum updated to improve speed.
  sum -= LM[index];
  LM[index] = M;
  sum += LM[index];
  index++;
  index = index % LM_SIZE;
  if (count < LM_SIZE) count++;
  _tft->print(" AVG:");
  _tft->println(sum/count);
  return sum / count;
}

int runninAngleAverage(int angle, TFT_eSPI *_tft) {

#define LM_SIZE 10
  static int LMX[LM_SIZE];  // LastMeasurements
  static int sumX = 0;

  static int LMY[LM_SIZE];  // LastMeasurements
  static int sumY = 0;

  static byte index = 0;
  static byte count = 0;

  // keep sum updated to improve speed.
  sumX -= LMX[index];
  LMX[index] = 100 * cos(angle * M_PI / 180);
  sumX += LMX[index];

  sumY -= LMY[index];
  LMY[index] = 100 * sin(angle * M_PI / 180);
  sumY += LMY[index];

  index++;
  index = index % LM_SIZE;
  if (count < LM_SIZE) count++;

/*
  for (i = 0; i < size; i++) {
    x_part += cos(angles[i] * M_PI / 180);
    y_part += sin(angles[i] * M_PI / 180);
  }
*/
  float avg = atan2(sumY/100.0/count, sumX/100.0/count) * 180 / M_PI;
  if (avg < 0) avg = avg + 360;
  _tft->println((int)avg);
  return (int)avg;
}

TFT_eSPI tft = TFT_eSPI();

void setup() {
  tft.init();
  tft.setRotation(1);
  tft.setTextSize(1);
  tft.fillScreen(TFT_BLACK);
 
}

int angle=0;
long randNum;

void loop() {
  randNum = random(100, 120);
  runninAngleAverage(randNum, &tft);
  delay(1000);
}