   }
 }
 
-//update solar panel values
-void updateSolar() {
-  for (int i = 0; i < 9; i++) {
-    solar[i] = analogRead(solar_pins[i]);
-    delay(50);
-  }
-}
-
-//display updated solar panel values
-void solarInfoDisplay() {
-  Serial.println("Solar Display: ");
-  for (int i = 0; i < 9; i++) {
-    Serial.print("Solar Pin ");
-    Serial.print(i);
-    Serial.print(" : ");
-    Serial.print(solar[i]);
-    Serial.print("     ");
-    delay(50);
-
-    Serial.println();
-  }
-}
+
 
 //update sonar sensor values in terms of distance
 void updateSonar() {
-  for (int i = 0; i < 6; i++) {
+  for (int i = 0; i < 1; i++) {
     //cm[i]=sonar[i].ping_cm();
     unsigned int uS = sonar[i].ping();
     cm[i] = sonar[i].convert_cm(uS);
     delay(50);
 
-    Serial.println(cm[i]);
+//    Serial.println(cm[i]);//
 
 
 

  }
*/
void loop() {
 // motospd(5,5,5);
//Pixy_Align();
//updateSonar();
sonarInfoDisplay();
//Do_Thingy();
//Go_To_Object();
//wheel_speeds(40,270);
//motospd(0,0,40);
  delay(1000);
}
void Do_Thingy(){
  boolean aligned = false;

  }
  delay(100);
}
float velocity = 24.6; //velocity in cm/s. !!!!!!.!!!!!NEED TO CALIBRATE!!!!!!!!!!!
time_t timerTime; //Time after Arduino is powered on that timing starts
time_t timerReading; //Time since timing starts
void velocity_calib(){
  updateSonar();
  double start_pos = cm[0];
  Serial.println(cm[0]);//
  timerReset();
  wheel_speeds(60,3);
 //motospd(-40,0,40);
  while(cm[0] >20){
    updateSonar();
    timerRead();
  }
  motospd(0,0,0);
  velocity = start_pos/(timerReading/1000);
  Serial.println(velocity);
  return;
}
void timerReset(){
    timerTime = millis();   
    timerReading = 0;
    }

void timerRead(){
  timerReading = millis()-timerTime;
}


float Compass = 0; //Angle the robot is facing, relative to the room with 0 deg straight back into room, assumes robot is facing 0 deg. 

float boxSize = 50; //Size of the gridsquares that make up the room.



void complexMove(int goalX, int goalY, float orientation){ //Robot winds up in a specified grid, facing a specified direction, measured in grid squares and degrees.  
  translateCart(goalX, goalY); //Robot moves to a specified location
  float dTheta = orientation - Compass;
  Compass = orientation; 
  rotate(dTheta); //Robot rotates to face a specified direction
}

void translatePolar(float azimuth, float destDistCm){ //Robot moves a specified distance and direction. 
  timerReset(); //Reset the timer. 
//  timerRead();
  float duration = 1000 * (destDistCm / velocity); //time needed to travel in ms.
  Serial.print("The Robot Will Now Move for ");
  Serial.print(duration/1000);
  Serial.println("Seconds");
  wheel_speeds(60,azimuth+360); //speed in "motospd units"
  Serial.println(timerReading);
  while (timerReading < duration){
    boxUpdate(azimuth,timerReading,velocity);
    timerRead(); 
    Serial.print("The Robot is in Box: ");
    Serial.print(boxLocation[0]);
    Serial.println(boxLocation[1]);
    delay(10);
  }
  Serial.println("The Robot has Stopped");
  motospd(0,0,0); //Stop the robot

}

void translateCart(int goalX, int goalY){ //Determines the distance and direction the robot needs to move to reach a specified grid (X,Y);
  if(boxLocation[0] == goalX && boxLocation[1] == goalY)
    return;
  int destBox[2] = {goalX, goalY};
  int deltaBox[2];//How many boxes right/left, up/down the robot needs to move to reach desired location.
  float deltaCm[2]; //Distance in cm that the robot needs to move up/down/left/right. 
  for (int i = 0; i < 2; i++) //Math
  {
    deltaBox[i] = destBox[i]-boxLocation[i];
    deltaCm[i] = boxSize*deltaBox[i];
  }
  float destDistCm = sqrt(pow((boxLocation[0]-goalX)*boxSize,2)+pow((boxLocation[1]-goalY)*boxSize,2));
  Serial.println("destDistCm:");
  Serial.println(destDistCm);
  float azimuth = atan2(deltaCm[1],deltaCm[0]);
  Serial.println("The crystal Gem is:");
  Serial.println(((azimuth*180)/PI)-90);
  translatePolar(((azimuth*180)/PI)-90, destDistCm); //Once the robot determines the distance and 
  boxLocation[0] = goalX;
  boxLocation[1]= goalY;
}

void rotate(float deg){ //input the angle you want to face, relative to the direction the robot is currently facing. 
  
  while (deg >= 360){ //puts angle in correct range
  deg = deg - 360;
  }
  
  while (deg <= -360){ //puts angle in correct range
  deg = deg + 360;
  }
  
  if (deg > 0 && deg <=180){
    motospd(30,30,30);
  }
  else if (deg <= -180 && deg >-360){
    deg = 360 + deg;
    motospd(30,30,30);
  }
  else if(deg >180 && deg < 360){
    deg = 360-deg; 
    motospd(-30,-30,-30);
  }
  else if (deg <0 && deg > -180){
    deg = -1*deg;
    motospd(-30,-30,-30);
  }
  else if (deg = 0) {
    motospd(0,0,0);
  }
  float runtime = 1000*(0.0276*deg - 0.2101); //time it takes to rotate in milliseconds, based on calibration data.
  unsigned long longTime = (unsigned long) runtime;
  delay(longTime);
  motospd(0,0,0);
  delay(100); //extra delay to avoid errors. 
  
}

void boxUpdate(float dirc, float dur, float spd){ //Take input direction, time, and speed of robot. Direction taken from some other function, time, from timer, and spd assumed to be absolute. 
//      boxLocation [0] = (int) (spd*dur*10*cos(dirc))/boxSize;
//      boxLocation [1] = (int) (spd*dur*10*sin(dirc))/boxSize;    
    }
























//look at pathfinding in the repo for comments and explanation for a* algo. Here is where i explain obstacle tracking




struct node grid[20][20];
int gridsizex = 20;

int gridsizey = 20;
void gridInit(){
  for (int i = 0; i < gridsizex; i++)
  {
    for (int j = 0; j < gridsizey; j++)
    {
      grid[i][j].posx = i;
      grid[i][j].posy = j;
      grid[i][j].walkable = true;
    }
  }
  grid[0][10].walkable = false;
  grid[0][9].walkable = false;
  grid[1][10].walkable = false;
  grid[2][9].walkable = false;


}

void show_Grid(){
  for(int i = 0; i <20; i++){
    for(int j = 0; j <20; j++){  
      if(grid[j][i].walkable && !grid[j][i].in_path){
        Serial.print("0 ");  
      }
      else if (grid[j][i].in_path)
        Serial.print("X ");
      else
        Serial.print("1 ");
      
    }
     Serial.println();
  }

}

//using structures to represent nodes for openList and closed lists
vector<struct node *> open; // nodes that need to be evaluated
vector<struct node *> closed; //already been evaluated

vector<struct node *> neighbours;




struct node * get_node(int x, int y){

  return &grid[x][y];

}

//calculate fcost
int fcost(struct node *a)
{
  return a->gcost + a->hcost;
}

int getDistance(struct node *a, struct node *b){
  int dstX = abs(a->posx - b->posx);
  int dstY = abs(a->posy - b->posy);

  return dstX + dstY;
}

bool inClosed(struct node *current)
{
  for(int i = 0; i< closed.size(); i++){
    if (current == closed[i])
      return true;
  }
  return false;
}

bool inOpen(struct node *current)
{
  for(int i = 0; i< open.size(); i++){
    if (current == open[i])
      return true;
  }
  return false;

}



void printnode(struct node* node){ // print this bitch
 Serial.print( node->posx);
  Serial.print(" ");
  Serial.println(node->posy);
}

void RetracePath(struct node *startNode, struct node *endNode){
 // cout << "[retrace]" << "\n";
  path.clear();
  struct node *currentNode = endNode;

  // TODO WTF DO WE IF THERES NO PATH BRAH
  while(currentNode != startNode){
      currentNode->in_path = true;
    path.push_back(currentNode);
    currentNode = currentNode->parent;
  Serial.print("[");
   Serial.print(currentNode->posx);
    Serial.print(", ");
     Serial.print(currentNode->posy);
      Serial.println("]");

    if (!currentNode) {
      // THERE'S NO PATH THO.
      break;


    }
  }
}
void GetNeighbours(struct node *current){
  neighbours.clear();

  for(int x = -1; x<=1;x++){ 
    for (int y = -1; y <= 1; y++){
      if(abs(x)==abs(y))
        continue;

      int checkX = current->posx + x;
      int checkY = current->posy + y;

      if(checkX >= 0 && checkX < gridsizex && checkY >= 0 && checkY<gridsizey){
        neighbours.push_back(&grid[checkX][checkY]);
      }
    }
  }
  return; 
}
void FindPath (int start_x, int start_y, int target_x, int target_y){
  struct node *startNode = get_node(start_x,start_y);
  struct node *targetNode = get_node(target_x,target_y);
  //cout << "[1]" << "\n";
  open.push_back(startNode);
  //cout << open[0]->posy << "\n";

 

  while(open.size() > 0 ){
    //cout << "[2]" << "\n";
     
    struct node *currentNode = open[0];
    int index = 0;

    for (int i =1; i<open.size(); i++){
      Serial.println("HIIHIHIHIHIHI");
      if(fcost(open[i]) < fcost(currentNode) || fcost(open[i]) == fcost(currentNode) && open[i]->hcost < currentNode->hcost){
        currentNode = open[i];
        index = i;
        
      }
      
    }
    open.erase(open.begin() + index);
    closed.push_back(currentNode);
   // printnode(currentNode);
    if(currentNode == targetNode){
      RetracePath(startNode, targetNode);
      return;
    }

    GetNeighbours(currentNode);
    for(int i = 0; i< neighbours.size(); i++){  
      if (!neighbours[i]->walkable || inClosed(neighbours[i])){
        continue;
      }
      int newMovementCostToNeighbour = currentNode->gcost + getDistance(currentNode,neighbours[i]);
      if(newMovementCostToNeighbour < neighbours[i]->gcost || !inOpen(neighbours[i])){
        neighbours[i]->gcost = newMovementCostToNeighbour;
        neighbours[i]->hcost = getDistance(neighbours[i], targetNode);
        neighbours[i]->parent = currentNode;

        if(!inOpen(neighbours[i]))
          open.push_back(neighbours[i]);
      }

    }
  }
} 


// this is where obstacle detection and avoidance starts

//dtermines if the locations detected by sonar are occupied or not
void update_grid(int x, int y, int target_x,int target_y){
  updateSonar();
  //distance of object detected
  float sonarleft = cm[0];

//adjust 150 to what you want, 150cm is what i consider accurate
  if(sonarleft <= 150 && sonarleft != 0){
    int unoccupied = sonarleft/25;
    //adjust 25 to what you want. i wanted each box in the grid to be 25x25cm. I will increase this to match the demensions of the robot
    
    //this is to remove obsticales that were there before, but were removed. marks true if no obstacle
    for (int u = 0; u <= unoccupied; u++){
      grid[x + u][y].walkable = true;
    }
    
    grid[x + unoccupied + 1][y].walkable = false;

  }
  //if there is no obsticale, mark the boxes as unoccupied.....so since 150cm is our range, range/nodesize = # of nodes 
  else{
    int unoccupied = 150/25;

      for (int u = 0; u <= unoccupied; u++){
      grid[x + u][y].walkable = true;
    }
  }
     
        

}
//resets in_path to false for all locations 
void reset_path() {
  for(int i = 0; i <20; i++){
    for(int j = 0; j <20; j++){  
     grid[i][j].in_path = false;
      }
    }
}
//erases open and closed list
void reset_lists(){
  open.clear();
  closed.clear();
}



