design_ex2_v2 (still not done)

#include <mpu6050_esp32.h>
#include <WiFi.h> //Connect to WiFi Network
#include <TFT_eSPI.h> // Graphics and font library for ST7735 driver chip
#include <SPI.h> //Used in support of TFT Display
#include <string.h>  //used for some string handling and processing.

TFT_eSPI tft = TFT_eSPI();  // Invoke library, pins defined in User_Setup.h

char network[] = "EECS_Labs";  //SSID for 6.08 Lab 2.4 GHz devices
char password[] = ""; //Password for 6.08 Lab 2.4 GHz devices
char dormnetwork[] = "MIT";
const char USER[] = "yimingz";


uint8_t state = 0;
#define IDLE 0  //example definition
#define PEAK 1  //example definition
#define TROUGH 2
#define BACKGROUND TFT_GREEN
#define BALL_COLOR TFT_BLUE


const int DT = 400; //milliseconds
const int EXCITEMENT = 10000; //how much force to apply to ball
const uint8_t BUTTON_PIN = 16; //CHANGE YOUR WIRING TO PIN 16!!! (FROM 19)
const int MAX_NUM_OF_COORDS = 1000;

uint32_t primary_timer; //main loop timer
char* drawing_id; // last drawing_id returned from post request

//storage of ball positions
float x_positions[MAX_NUM_OF_COORDS];
float y_positions[MAX_NUM_OF_COORDS];
int ball_position_index = 0;
char ball_position_buffer[5*MAX_NUM_OF_COORDS];
int ball_position_buffer_counter = 0;
int coords[2*MAX_NUM_OF_COORDS];
int num_of_coords = 0;


//positions, velocities, and accelerations:
float x_pos = 64; //x position
float y_pos = 32; //y position
float x_vel = 0; //x velocity
float y_vel = 0; //y velocity
float x_accel = 0; //x acceleration
float y_accel = 0; //y acceleration
float x,y,z; //variables for grabbing x,y,and z values


//physics constants:
const float MASS = 1; //for starters
const int RADIUS = 2; //radius of ball
const float K_FRICTION = 0.15;  //friction coefficient
const float K_SPRING = 0.9;  //spring coefficient

//boundary constants:
const int LEFT_LIMIT = RADIUS; //left side of screen limit
const int RIGHT_LIMIT = 127-RADIUS; //right side of screen limit
const int TOP_LIMIT = RADIUS; //top of screen limit
const int BOTTOM_LIMIT = 159-RADIUS; //bottom of screen limit

//IMU constants:
const float resting_x_acc = 0.05;
const float resting_y_acc = 0.03;

//HTTP request constants
const int RESPONSE_TIMEOUT = 6000; //ms to wait for response from host
const int POSTING_PERIOD = 20000; //periodicity of getting a number fact.
const int GETTING_PERIOD = 10000;

const uint16_t IN_BUFFER_SIZE = 1000; //size of buffer to hold HTTP request
const uint16_t OUT_BUFFER_SIZE = 1000; //size of buffer to hold HTTP response
char request_buffer[IN_BUFFER_SIZE]; //char array buffer to hold HTTP request
char response_buffer[OUT_BUFFER_SIZE]; //char array buffer to hold HTTP response
char get_response_buffer[OUT_BUFFER_SIZE];

bool pushed_last_time; //for finding change of button (using bool type...same as uint8_t)
uint32_t posting_timer = 0;
uint32_t last_time = 0;
bool havePosted = false;

float old_acc[3]; // array with x,y,z accelerations
float older_acc[3];
MPU6050 imu; //imu object called, appropriately, imu

void step(float x_force=0, float y_force=0 ){
  //update acceleration (from f=ma)
  x_accel = (x_force-K_FRICTION*x_vel)/MASS;
  y_accel = (y_force-K_FRICTION*y_vel)/MASS;
  //integrate to get velocity from current acceleration
  x_vel = x_vel + 0.001*DT*x_accel; //integrate, 0.001 is conversion from milliseconds to seconds
  y_vel = y_vel + 0.001*DT*y_accel; //integrate!!
  //
  moveBall();
}

void moveBall(){
  if(x_pos + 0.001*DT*x_vel > RIGHT_LIMIT) {
    x_pos = RIGHT_LIMIT - K_SPRING*(x_pos + 0.001*DT*x_vel - RIGHT_LIMIT);
    x_vel = -K_SPRING*x_vel;
  }
  else if(x_pos + 0.001*DT*x_vel < LEFT_LIMIT) {
    x_pos = LEFT_LIMIT + K_SPRING*(LEFT_LIMIT - (x_pos + 0.001*DT*x_vel));
    x_vel = -K_SPRING*x_vel;
  }
  else {
    x_pos = x_pos + 0.001*DT*x_vel;
  }
  if(y_pos + 0.001*DT*y_vel > BOTTOM_LIMIT) {
    y_pos = BOTTOM_LIMIT - K_SPRING*(y_pos + 0.001*DT*y_vel - BOTTOM_LIMIT);
    y_vel = -K_SPRING*y_vel;
  }
  else if(y_pos + 0.001*DT*y_vel < TOP_LIMIT) {
    y_pos = TOP_LIMIT + K_SPRING*(TOP_LIMIT - (y_pos + 0.001*DT*y_vel));
    y_vel = -K_SPRING*y_vel;
  }
  else {
    y_pos = y_pos + 0.001*DT*y_vel;
  }
}


void setup() {
  Serial.begin(115200); //for debugging if needed.
  pinMode(BUTTON_PIN,INPUT_PULLUP);
  tft.init();
  tft.setRotation(2);
  tft.setTextSize(1);
  tft.fillScreen(BACKGROUND);
  Serial.begin(115200); //begin serial comms
  delay(100); //wait a bit (100 ms)

  WiFi.begin(dormnetwork,password); //attempt to connect to wifi
  uint8_t count = 0; //count used for Wifi check times
  Serial.print("Attempting to connect to ");
  Serial.println(network);
  Wire.begin();
  delay(50); //pause to make sure comms get set up
  if (imu.setupIMU(1)){
    Serial.println("IMU Connected!");
  }else{
    Serial.println("IMU Not Connected :/");
    Serial.println("Restarting");
    ESP.restart(); // restart the ESP (proper way)
  }
  while (WiFi.status() != WL_CONNECTED && count<12) {
    delay(500);
    Serial.print(".");
    count++;
  }
  delay(2000);
  if (WiFi.isConnected()) { //if we connected then print our IP, Mac, and SSID we're on
    Serial.println("CONNECTED!");
    Serial.println(WiFi.localIP().toString() + " (" + WiFi.macAddress() + ") (" + WiFi.SSID() + ")");
    delay(500);
  } else { //if we failed to connect just Try again.
    Serial.println("Failed to Connect :/  Going to restart");
    Serial.println(WiFi.status());
    ESP.restart(); // restart the ESP (proper way)
  }

  x_positions[0] = (int) x_pos;
  y_positions[0] = (int) y_pos;
  ball_position_buffer_counter = snprintf(ball_position_buffer, 5*MAX_NUM_OF_COORDS, "%d,%d&", (int) x_pos, (int) y_pos);

  step(); // apply no initial force
  pushed_last_time = false;
  primary_timer = millis();
}

void loop() {
  ball_position_index++; // Increment first since we already did index = 0 in setup()
  x_positions[ball_position_index] = x_pos;
  y_positions[ball_position_index] = y_pos;
  imu.readAccelData(imu.accelCount);
  x = imu.accelCount[0]*imu.aRes;
  y = imu.accelCount[1]*imu.aRes;
  z = imu.accelCount[2]*imu.aRes;

  float acc[] = {x,y,z};
  float avg_acc[3];

  for(int i=0; i<3; i++) {
    avg_acc[i] = (acc[i] + old_acc[i] + older_acc[i])/3;
  }

  for(int i=0; i<3; i++) {
    older_acc[i] = old_acc[i];
    old_acc[i] = acc[i];
  }

  step((avg_acc[0]-resting_x_acc) * 100 * MASS, (avg_acc[1]-resting_y_acc) * 100 * MASS);
  tft.fillCircle(x_pos,y_pos,RADIUS,BALL_COLOR); //draw new ball location

  if((ball_position_buffer_counter >= 0 && ball_position_buffer_counter < 5*MAX_NUM_OF_COORDS) && ball_position_index<(sizeof(x_positions)/sizeof(int))) {
    ball_position_buffer_counter += snprintf(ball_position_buffer+ball_position_buffer_counter, 5*MAX_NUM_OF_COORDS-ball_position_buffer_counter, "%d,%d&", (int) x_positions[ball_position_index], (int) y_positions[ball_position_index]);
  }

  // TESTING ONLY
  // post request test
  if (millis()-posting_timer > POSTING_PERIOD) {
    char body[5000]; //for body
    sprintf(body,"{\"user\":\"%s\",\"drawing\":\"%s\"}",USER,ball_position_buffer);//generate body, posting to User, 1 step
    int body_len = strlen(body); //calculate body length (for header reporting)
    sprintf(request_buffer,"POST http://608dev.net//sandbox/etchsketch HTTP/1.1\r\n");
    strcat(request_buffer,"Host: 608dev.net\r\n");
    strcat(request_buffer,"Content-Type: application/json\r\n");
    sprintf(request_buffer+strlen(request_buffer),"Content-Length: %d\r\n", body_len); //append string formatted to end of request buffer
    strcat(request_buffer,"\r\n"); //new line from header to body
    strcat(request_buffer,body); //body
    strcat(request_buffer,"\r\n"); //header
    //Serial.println(request_buffer);
    do_http_request("608dev.net", request_buffer, response_buffer, OUT_BUFFER_SIZE, RESPONSE_TIMEOUT,true);
    //Serial.println("RESPONSE!!!");
    //Serial.println(response_buffer);
    char token[5];
    sprintf(token,"%c",':');
    char* ptr = strtok(response_buffer, token);
    ptr = strtok(NULL, token);
    drawing_id = ptr;
    Serial.println("DRAWING ID IN POST");
    Serial.println(drawing_id);
    memset(request_buffer, 0, strlen(request_buffer));

    posting_timer = millis();
    havePosted = true;
  }

  // get request test
  if(millis() - last_time > GETTING_PERIOD) {
    if(havePosted) {
       Serial.println("DRAWING ID FOR GET REQUEST");
       Serial.println(drawing_id);
       sprintf(request_buffer,"GET http://608dev.net//sandbox/etchsketch?drawing_id=%s HTTP/1.1\r\n", drawing_id);
       strcat(request_buffer,"Host: 608dev.net\r\n"); //add more to the end
       strcat(request_buffer,"\r\n");
       do_http_request("608dev.net", request_buffer, get_response_buffer, OUT_BUFFER_SIZE, RESPONSE_TIMEOUT, true);
       Serial.println(get_response_buffer);

       char token[5];
       sprintf(token,"%c",'~');
       char* ptr = strtok(get_response_buffer, token);
       ptr = strtok(NULL, token);
       strcpy(ball_position_buffer,ptr);
       Serial.println("BALL POSITION BUFFER");
       Serial.println(ball_position_buffer);

       memset(x_positions, 0, MAX_NUM_OF_COORDS);
       memset(y_positions, 0, MAX_NUM_OF_COORDS);
       memset(coords, 0, 2*MAX_NUM_OF_COORDS);
       // GET REQUEST
       int_extractor(ball_position_buffer, coords);
       Serial.println("COORDS");
       for(int i=0; i<100; i++) {
        Serial.println(coords[i]);
       }
       num_of_coords = 0;
       for(int i=0; i<MAX_NUM_OF_COORDS; i++) {
          if(coords[i] <= 159 && coords[i] > 0) {
              num_of_coords++;
          }
          else {
            break;
          }
        }

        num_of_coords /= 2;

        Serial.println("NUM OF COORDS!!!!");
        Serial.println(num_of_coords);

        // Clear background, update x,y arrays, and draw the balls
        tft.fillScreen(BACKGROUND);
        for(int i=0; i<num_of_coords; i++) {
          x_positions[i] = coords[2*i];
          y_positions[i] = coords[2*i+1];
          tft.fillCircle(x_positions[i],y_positions[i],RADIUS,BALL_COLOR);
        }

       ball_position_index = num_of_coords-1;
       ball_position_buffer_counter = 0;
       // TODO FIGURE OUT ball_position_buffer_counter
    }
    else {
      Serial.println("ERROR: NO DRAWING ID");
    }
      last_time = millis();
  }

  while (millis()-primary_timer < DT); //wait for primary timer to increment
  primary_timer = millis();
}


// Int extractor function (from Ex 2) with both ',' and '&' as delimiters
void int_extractor(char* data_array, int* output_values){
  if(sizeof(data_array)/sizeof(char) == 0) {
      return;
  }
  int ind=0;
  char token[5];
  sprintf(token,"%c%c",',','&');
  char* ptr = strtok(data_array, token);
  if(ptr == NULL) {
    return;
  }
  output_values[ind] = atoi(ptr);
  ind++;
  while(ptr != NULL) {
    ptr = strtok(NULL, token);
    if(ptr == NULL) {
        break;
    }
    output_values[ind] = atoi(ptr);
    ind++;
  }
}


/*----------------------------------
 * char_append Function:
 * Arguments:
 *    char* buff: pointer to character array which we will append a
 *    char c:
 *    uint16_t buff_size: size of buffer buff
 *
 * Return value:
 *    boolean: True if character appended, False if not appended (indicating buffer full)
 */
uint8_t char_append(char* buff, char c, uint16_t buff_size) {
        int len = strlen(buff);
        if (len>buff_size) return false;
        buff[len] = c;
        buff[len+1] = '\0';
        return true;
}

/*----------------------------------
 * do_http_request Function:
 * Arguments:
 *    char* host: null-terminated char-array containing host to connect to
 *    char* request: null-terminated char-arry containing properly formatted HTTP request
 *    char* response: char-array used as output for function to contain response
 *    uint16_t response_size: size of response buffer (in bytes)
 *    uint16_t response_timeout: duration we'll wait (in ms) for a response from server
 *    uint8_t serial: used for printing debug information to terminal (true prints, false doesn't)
 * Return value:
 *    void (none)
 */
void do_http_request(char* host, char* request, char* response, uint16_t response_size, uint16_t response_timeout, uint8_t serial){
  WiFiClient client; //instantiate a client object
  if (client.connect(host, 80)) { //try to connect to host on port 80
    if (serial) Serial.print(request);//Can do one-line if statements in C without curly braces
    client.print(request);
    memset(response, 0, response_size); //Null out (0 is the value of the null terminator '\0') entire buffer
    uint32_t count = millis();
    while (client.connected()) { //while we remain connected read out data coming back
      client.readBytesUntil('\n',response,response_size);
      if (serial) Serial.println(response);
      if (strcmp(response,"\r")==0) { //found a blank line!
        break;
      }
      memset(response, 0, response_size);
      if (millis()-count>response_timeout) break;
    }
    memset(response, 0, response_size);
    count = millis();
    while (client.available()) { //read out remaining text (body of response)
      char_append(response,client.read(),OUT_BUFFER_SIZE);
    }
    if (serial) Serial.println(response);
    client.stop();
    if (serial) Serial.println("-----------");
  }else{
    if (serial) Serial.println("connection failed :/");
    if (serial) Serial.println("wait 0.5 sec...");
    client.stop();
  }
}