send

#include "simpletools.h"
#include "ping.h"
#include "abdrive.h"
#define PAUSE 950
#define speed 50
#define pid_fac 2
#define SIDE 40
#define NORTH 0
#define EAST 1
#define SOUTH 2
#define WEST 3
#define cmToTicks (1 / 0.325)

int leftDist, rightDist;
void irVal() {
    leftDist = 0;
    rightDist = 0;
    for(int dacVal = 0; dacVal < 160; dacVal += 8) {
        dac_ctr(26, 0, dacVal);
        freqout(11, 1, 38000);
        leftDist += input(10);
        dac_ctr(27, 1, dacVal);
        freqout(1, 1, 38000);
        rightDist += input(2);
    }
}

int nodeAt = 1;
int destination;
int present_direction = NORTH;
int result_length;
int old_ticks_left = 0, old_ticks_right = 0;
int leftTicks = 0, rightTicks = 0;

void turn(char* str){
    if(str == "left"){
        switch (present_direction) {
            case NORTH:
            present_direction = WEST;
            break;
            case WEST:
            present_direction = SOUTH;
            break;
            case SOUTH:
            present_direction = EAST;
            break;
            default:
            present_direction = NORTH;
        }
    }
    else {
        switch (present_direction) {
            case NORTH:
            present_direction = EAST;
            break;
            case EAST:
            present_direction = SOUTH;
            break;
            case SOUTH:
            present_direction = WEST;
            break;
            default:
            present_direction = NORTH;
        }
    }
}

typedef struct Node {
    struct Node *next;
    struct Node *prev;
    int nodeNumber;
} Node;

typedef struct List {
    struct Node *first;
    struct Node *last;
    int numberOfNodes;
} List;

List *list;
List *reverse_path_list;

int lengthOfList(List *temp) {
    Node *head = temp -> first;
    int len = 0;
    while(head != NULL) {
        len++;
        head = head -> next;
    }
    return len;
}

void shortestPath(List *list1) {
    int length = lengthOfList(list1);
    Node *temp1 = list1 -> first;
    for(int i = 0; temp1 != NULL; i++) {
        Node *temp2 = temp1 -> next;
        for(int j = i + 1; temp2 != NULL; j++) {
            if(temp1 -> nodeNumber == temp2 -> nodeNumber) {
                temp1 -> next = temp2 -> next;
            }
            temp2 = temp2 -> next;
        }
        temp1 = temp1 -> next;
    }
}

int* toArray(List *list1) {
    result_length = lengthOfList(list1) + 1;
    Node *temp = list1 -> first;
    int *arr = (int*) malloc (sizeof(int) * result_length);
    arr[0] = -3;
    for(int i = 1; i < result_length; i++) {
        arr[i] = temp -> nodeNumber;
        temp = temp -> next;
    }
    return arr;
}

void prependNode() {
    Node *node = (Node*)malloc(sizeof(Node));
    node -> next = NULL;
    node -> prev = NULL;
    node -> nodeNumber = nodeAt;
    if (reverse_path_list -> last == NULL) {
        reverse_path_list -> first = node;
        reverse_path_list -> last = node;
    } else {
        node -> next = reverse_path_list -> first;
        reverse_path_list -> first -> prev = node;
        reverse_path_list -> first = node;
    }
    reverse_path_list -> numberOfNodes += 1;
}

void appendNode() {
    Node *node = (Node*)malloc(sizeof(Node));
    node -> next = NULL;
    node -> prev = NULL;
    node -> nodeNumber = nodeAt;
    if (list -> first == NULL) {
        list -> first = node;
        list -> last = node;
    } else {
        list -> last -> next = node;
        list -> last = node;
    }
    list -> numberOfNodes += 1;
}

void moveForward() {
    switch(present_direction) {
        case NORTH:
        nodeAt += 4;
        break;
        case EAST:
        nodeAt += 1;
        break;
        case WEST:
        nodeAt -= 1;
        break;
        default:
        nodeAt -= 4;
    }
    if(destination == 16 || destination == 13){
        appendNode();
    }
    if(destination == 1 || destination == 4){
        prependNode();
    }
    move_a_block();
}

void goto_node() {
    while(nodeAt != destination) {
        drive_goto(-25, 26);
        if (ping_cm(8) >= 30) {
            turn("left");
            moveForward();
        } else {
            drive_goto(25, -26);
            if(ping_cm(8) <= 30) {
                drive_goto(25, -26);
                turn("right");
                if(ping_cm(8) <= 30) {
                    turn("right");
                    drive_goto(25, -25);
                    moveForward();
                }
                else {
                    moveForward();
                }
            }
            else {
                moveForward();
            }
        }
    }
}

void move_a_block_helper() {
    int error;
    if(leftDist < 20 && rightDist < 20 && ((leftDist - rightDist) >= 2 || (leftDist - rightDist) <= -2)) {
        error = leftDist - rightDist;
    }
    else if (rightDist < 20 && leftDist == 20 && ((15 - rightDist) >= 2 || (15 - rightDist) <= -2)) {
        error = 15 - rightDist;
    }
    else if (rightDist == 20 && leftDist < 20 && ((leftDist - 15) >= 2 || (leftDist - 15) <= -2)) {
        error = leftDist - 15;
    }
    else {
        error = 0;
    }
    error *= pid_fac;
    if(error < 0) {
        drive_speed(speed - error, speed);
    }
    else {
        drive_speed(speed, speed + error);
    }
}

void move_a_block() {
    drive_getTicks(&leftTicks, &rightTicks);
    old_ticks_left = leftTicks;
    old_ticks_right = rightTicks;
    while((leftTicks - old_ticks_left < (SIDE * cmToTicks))
      && (rightTicks - old_ticks_right < (SIDE * cmToTicks))) {
        irVal();
        move_a_block_helper();
        drive_getTicks(&leftTicks, &rightTicks);
    }
    drive_speed(0, 0);
}

void turnTo(int direction){
  drive_speed(0, 0);
  int diff = present_direction - direction;
  drive_goto(-(25 * diff), (26 * diff));
  present_direction = direction;
  drive_speed(128, 128);
  return;
}

int nextDiff(int input[], int index, int size){
  if (index >= size - 1){return -1;}
  int curr = input[index];
  int target = input[index + 1];
  int diff = target - curr;
  return diff;
}

void move(int input[], int index, int size){
  int diff = nextDiff(input, index, size);
  if(diff == -1){
    drive_speed(0, 0);
    return;
  }
  int counter = 0;
  switch (diff){
    case 1:
      turnTo(EAST);
      counter = 0;
      while(diff == 1){
        ++index;
        counter++;
        diff = nextDiff(input, index, size);
      }
      drive_speed(128, 128);
      pause(counter * PAUSE);
      break;
    case -1:
      turnTo(WEST);
      counter = 0;
      while(diff == -1){
        ++index;
        counter++;
        diff = nextDiff(input, index, size);
      }
      drive_speed(128, 128);
      pause(counter * PAUSE);
      break;
    case 4:
      turnTo(NORTH);
      counter = 0;
      while(diff == 4){
        ++index;
        counter++;
        diff = nextDiff(input, index, size);
      }
      drive_speed(128, 128);
      pause(counter * PAUSE);
      break;
    default:
      turnTo(SOUTH);
      counter = 0;
      while(diff == -4){
        ++index;
        counter++;
        diff = nextDiff(input, index, size);
      }
      drive_speed(128, 128);
      pause(counter * PAUSE);
  }
  move(input, index, size);
}

int main() {
    list = malloc(sizeof(List));
    list -> first = NULL;
    list -> last = NULL;
    list -> numberOfNodes = 0;
    reverse_path_list = malloc(sizeof(List));
    reverse_path_list -> first = NULL;
    reverse_path_list -> last = NULL;
    reverse_path_list -> numberOfNodes = 0;

    move_a_block();
    appendNode();

    destination = 13;
    goto_node();

    destination = 16;
    goto_node();
    prependNode();
    destination = 4;
    goto_node();
    destination = 1;
    goto_node();

    shortestPath(list);
    shortestPath(reverse_path_list);

    int *result;
    int len;
    if(lengthOfList(list) <= lengthOfList(reverse_path_list)) {
        result = toArray(list);
    }
    else {
        result = toArray(reverse_path_list);
    }

    if (ping_cm(8) >= 30) {
        drive_goto(cmToTicks * SIDE, cmToTicks * SIDE);
    }
    else {
        drive_goto(-25, 26);
        drive_goto(cmToTicks * SIDE, cmToTicks * SIDE);
    }

    drive_goto(51, -51);
    present_direction = NORTH;

    for (int i = 0; i < 3; i++) {
        high(26);
        pause(250);
        low(26);
        pause(250);
    }

    drive_speed(128, 128);
    move(result, 0, result_length);
    drive_speed(0, 0);
}