//com37 

#include "painlessMesh.h"

HardwareSerial MySerial(0);   //Create a new HardwareSerial class.

#define BITMASK_MARKER 0b10000000 // 0b - binary (1,2,4...)
#define BITMASK_DATA 0b01111111
#define X_AXIS 500 // mm
#define Y_AXIS 550 // mm
#define Z_AXIS 460 // mm
#define XY_BUFFER 20 // mm
#define Z_BUFFER 80 // mm
#define SPINNER_DIAMETER 27 // mm
#define NUM_STEPS 800 

// for communication
#define   MESH_PREFIX     "orb"
#define   MESH_PASSWORD   "password"
#define   MESH_PORT       5555

Scheduler userScheduler; // to control your personal task
painlessMesh  mesh;


// Needed for painless library
void receivedCallback(uint32_t from, String &msg)  {

  String BUTTON = msg.substring(0,1);
  int BUTTON = int(BUTTON);

}
void newConnectionCallback(uint32_t nodeId) {
  Serial.printf("--> startHere: New Connection, nodeId = %u\n", nodeId);
}
void changedConnectionCallback() {
  Serial.printf("Changed connections\n");
}
void nodeTimeAdjustedCallback(int32_t offset) {
    Serial.printf("Adjusted time %u. Offset = %d\n", mesh.getNodeTime(),offset);
}

void setup() {
  // initialize both serial ports:
  Serial.begin(115200);
  MySerial.begin(115200, SERIAL_8N1, RX, TX); // at CPU Freq is 40MHz, work half speed of defined.

  mesh.setDebugMsgTypes( ERROR | STARTUP );  // set before init() so that you can see startup messages
  mesh.init(MESH_PREFIX, MESH_PASSWORD, &userScheduler, MESH_PORT );
  mesh.onReceive(&receivedCallback);
  mesh.onNewConnection(&newConnectionCallback);
  mesh.onChangedConnections(&changedConnectionCallback);
  mesh.onNodeTimeAdjusted(&nodeTimeAdjustedCallback);

  int STEPS_PER_SPIN = SPINNER_DIAMETER / NUM_STEPS;

  int CORNER_1_X = X_AXIS - XY_BUFFER;
  int CORNER_1_Y = Y_AXIS - XY_BUFFER;
  int CORNER_1_Z = Z_AXIS - Z_BUFFER;
  
  int CORNER_2_X = X_AXIS - XY_BUFFER;
  int CORNER_2_Y = Z_AXIS - XY_BUFFER;
  int CORNER_2_Z = Z_AXIS - Z_BUFFER;
  
  int CORNER_3_X = X_AXIS - XY_BUFFER;
  int CORNER_3_Y = Y_AXIS - XY_BUFFER;
  int CORNER_3_Z = Z_AXIS - Z_BUFFER;
  
  int CORNER_4_X = X_AXIS - XY_BUFFER;
  int CORNER_4_Y = Y_AXIS - XY_BUFFER;
  int CORNER_4_Z = Z_AXIS - Z_BUFFER;

  int CURRENT_POS_X = 0;
  int CURRENT_POS_Y = 0;
  int CURRENT_POS_Z = 0;
}

void send_message(int addr, int16_t pos, uint16_t delta) {
  MySerial.write(addr | BITMASK_MARKER);
  MySerial.write(((pos >> 7) & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write((pos & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write(((delta >> 7) & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write((delta & BITMASK_DATA) | BITMASK_MARKER);
  MySerial.write('\x00');
}

void loop() {
  // read from port 1, send to port 0:
  Serial.println("hello world");

  uint8_t value = 57;
  uint8_t b = BITMASK_MARKER | value;

  mesh.update();

  // send_message(0, 0, 4000); // 100 x 4000 should be microseconds of time to the complete task 
  // send_message(1, 0, 4000); // 100 x 4000 should be microseconds of time to the complete task 
  // send_message(2, 0, 4000); // 100 x 4000 should be microseconds of time to the complete task 
  // send_message(3, 0, 4000); // 100 x 4000 should be microseconds of time to the complete task 
  // delay(2000);
  // send_message(0, 500, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  // send_message(1, 500, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  // send_message(2, 500, 2000); // 100 x 4000 should be microseconds of time to the complete task 
  // send_message(3, 500, 2000); // 100 x 10000 should be microseconds of time to the complete task 
  // delay(2000);
  if(BUTTON == 1){ //LEFT
    int TARGET_POS_Y = -1; //mm
  }
    if(BUTTON == 2){ // RIGHT
    int TARGET_POS_Y = 1; //mm
  }
    if(BUTTON == 3){ //UP
    int TARGET_POS_Z = 1; //mm
  }
    if(BUTTON == 4){ //DOWN
    int TARGET_POS_Z = -1; //mm
  }
    if(BUTTON == 5){ //FORWARD
    int TARGET_POS_X = 1; //mm
  }
    if(BUTTON == 6){ //BACK
    int TARGET_POS_X = -1; //mm
  }


  }
}