/******************************************************************************

Torre di Hanoi

*******************************************************************************/
#include <stdio.h>

void hanoi(int num_disks, int start_pin, int end_pin, int temp_pin) {
    if (num_disks == 1) {
        printf("%d -> %d\n", start_pin, end_pin);
    } else {
        hanoi(num_disks-1, start_pin, temp_pin, end_pin);
        
        printf("%d -> %d\n", start_pin, end_pin);
        
        hanoi(num_disks-1, temp_pin, end_pin, start_pin);
    }   
}

int main() {
    hanoi(3, 1, 3, 2);

    return 0;
}

#include <webots/robot.h>
#include <webots/supervisor.h>
#include <stdio.h>

#define NUM_DISKS 4

WbFieldRef pos_fields[NUM_DISKS];

int disks[NUM_DISKS][3];

double x_pos[] = {-0.23, 0.02, 0.24};

int timestep;

void delay_ms(int ms, int timestep) {
  int steps = ms / timestep;
  for (int i = 0; i < steps; ++i)
    wb_robot_step(timestep);
}

void init() {
  WbNodeRef disk1 = wb_supervisor_node_get_from_def("DISK1");
  WbNodeRef disk2 = wb_supervisor_node_get_from_def("DISK2");
  WbNodeRef disk3 = wb_supervisor_node_get_from_def("DISK3");
  WbNodeRef disk4 = wb_supervisor_node_get_from_def("DISK4");

  pos_fields[0] = wb_supervisor_node_get_field(disk1, "translation");
  pos_fields[1] = wb_supervisor_node_get_field(disk2, "translation");
  pos_fields[2] = wb_supervisor_node_get_field(disk3, "translation");
  pos_fields[3] = wb_supervisor_node_get_field(disk4, "translation");
}

void initial_position() {
  double pos1[3] = {x_pos[0], 0.02, 0.01};
  double pos2[3] = {x_pos[0], 0.02, 0.03};
  double pos3[3] = {x_pos[0], 0.02, 0.05};
  double pos4[3] = {x_pos[0], 0.02, 0.07};
  wb_supervisor_field_set_sf_vec3f(pos_fields[0], pos1);
  wb_supervisor_field_set_sf_vec3f(pos_fields[1], pos2);
  wb_supervisor_field_set_sf_vec3f(pos_fields[2], pos3);
  wb_supervisor_field_set_sf_vec3f(pos_fields[3], pos4);
  
  int a=0;
  for (int r=0; r<NUM_DISKS; ++r) {
    disks[r][0] = a++;
    disks[r][1] = -1; 
    disks[r][2] = -1;
  }
}

void animate_disk_parabolic(int disk_id, double fromX, double toX, double fromZ, double toZ, double baseY, int timestep) {
  //const int steps = 25;
  
  // tempo costante
  int duration_ms = 600;
  int steps = duration_ms / timestep;
  if (steps < 2) steps = 2;  // evitare divisione per zero o salti brutti


  const double peak = 0.05;  // altezza massima sopra la linea retta Z

  for (int i = 0; i <= steps; ++i) {
    double t = (double)i / steps;

    // Interpolazione lineare X e Z
    double x = fromX + (toX - fromX) * t;
    double z_linear = fromZ + (toZ - fromZ) * t;

    // Parabola simmetrica con offset
    double z = z_linear + (4 * peak * t * (1 - t));  // massimo a t=0.5

    double pos[3] = {x, baseY, z};
    wb_supervisor_field_set_sf_vec3f(pos_fields[disk_id], pos);
    wb_robot_step(timestep);
  }
}

void move_disk(int from_pin, int to_pin) {
  int numFromDisks=0;
  for (int r=0; r<NUM_DISKS; ++r) {
    if (disks[r][from_pin] != -1) ++numFromDisks;
  }
  
  int numToDisks=0;
  for (int r=0; r<NUM_DISKS; ++r) {
    if (disks[r][to_pin] != -1) ++numToDisks;
  }
  
  int lastDisk=0;
  int lastDiskPos=0;
  for (int r=0; r<NUM_DISKS; ++r) {
    if (disks[r][from_pin] == -1) break;
    lastDisk = disks[r][from_pin];
    lastDiskPos = r;
  }
  
  double heigth = 0.01 + 0.02 * numToDisks;
  
  printf("numFromDisks=%d  numToDisks=%d\n", numFromDisks, numToDisks);
  
  //double new_pos[3] = {x_pos[to_pin], 0.02, heigth};
  //wb_supervisor_field_set_sf_vec3f(pos_fields[lastDisk], new_pos);
  
  double fromX = x_pos[from_pin];
  double toX = x_pos[to_pin];
  double fromZ = 0.01 + 0.02 * numFromDisks;
  double toZ = heigth;
  
  animate_disk_parabolic(lastDisk, fromX, toX, fromZ, toZ, 0.02, timestep);
  
  
  disks[lastDiskPos][from_pin] = -1;
  disks[numToDisks][to_pin] = lastDisk;
}

void hanoi(int num_disks, int start_pin, int end_pin, int temp_pin) {
    if (num_disks == 1) {
        printf("%d -> %d\n", start_pin, end_pin);
        move_disk(start_pin-1, end_pin-1);
        delay_ms(1000, timestep);
    } else {
        hanoi(num_disks-1, start_pin, temp_pin, end_pin);
        printf("%d -> %d\n", start_pin, end_pin);
        move_disk(start_pin-1, end_pin-1);
        delay_ms(1000, timestep);
        hanoi(num_disks-1, temp_pin, end_pin, start_pin);
    }
}

int main() {
  wb_robot_init();
  timestep = (int)wb_robot_get_basic_time_step();
  
  init();
  initial_position();
  
  delay_ms(1000, timestep);
  
  /*move_disk(0,1);
  
  delay_ms(1000, timestep);
  
  move_disk(0,1);]*/
  
  hanoi(4,1,3,2);
  
  wb_robot_cleanup();
  return 0;
}