#include <LinkedList.h>
#include <Servo.h>
int i, j, k, r, c, x, t;

// potentiometer pins
#define BASE_POT_PIN A0
#define SHOULDER_POT_PIN A2
#define SHOULDER2_POT_PIN A3
#define ELBOW_POT_PIN A1

// servo pins
#define BASE_SERVO_PIN 6
#define SHOULDER_SERVO_PIN 8
#define SHOULDER2_SERVO_PIN 7
#define ELBOW_SERVO_PIN 9

// motion constants
#define MOTION_DELAY 15

// servo objects
Servo base_servo, shoulder_servo, shoulder2_servo, elbow_servo;

// history of servo positions
LinkedList<int> *base_history = new LinkedList<int>();
LinkedList<int> *shoulder_history = new LinkedList<int>();
LinkedList<int> *shoulder2_history = new LinkedList<int>();
LinkedList<int> *elbow_history = new LinkedList<int>();



int a[4][9][8]=
 {
   {
    {48,44,40,38,34,30,27,22},
    {39,39,38,37,36,35,34,33},
    {49,45,42,39,35,31,26,22},
    {48,46,43,39,37,31,27,22},
    {48,46,42,39,37,31,28,22},
    {48,45,42,38,35,31,27,22},
    {48,44,42,38,35,30,26,22},
    {48,45,42,38,34,30,26,22},
    {48,45,42,37,33,29,26,22}
   },
   {
    {40,40,42,42,42,42,42,40},
    {46,46,47,46,46,46,46,45},
    {49,49,50,54,53,53,53,52},
    {56,56,54,57,54,57,57,55},
    {58,58,60,58,59,59,59,58},
    {62,62,64,64,64,64,56,60},
    {63,63,65,67,67,67,65,62},
    {63,65,67,68,68,66,66,62},
    {62,62,66,68,68,67,63,59}
   },
   {
    {140,140,138,138,138,138,138,140},
    {134,134,133,134,134,134,134,134},
    {131,131,130,126,127,127,127,128},
    {124,124,126,123,121,123,123,125},
    {122,122,120,122,120,121,121,122},
    {118,118,116,116,116,116,124,120},
    {117,118,115,113,113,113,115,118},
    {117,115,113,112,112,114,114,118},
    {118,118,113,112,112,113,117,121}
  },
  {
    {169,170,169,173,171,171,171,162},
    {171,171,171,171,171,171,171,171},
    {170,170,171,178,175,178,176,173},
    {174,176,175,178,171,178,177,173},
    {173,173,175,175,173,175,175,172},
    {171,170,175,169,176,176,166,168},
    {166,178,169,170,172,170,175,162},
    {160,167,170,172,172,169,164,161},
    {156,156,163,165,165,162,158,149}
  }
};
void loopthrough (int values[4][9][8]){
  int smiles [9][8]={
    {0,0,0,0,0,0,0,0},
    {1,1,1,1,1,1,1,1},
    {0,0,0,0,0,0,0,0},
    {0,0,0,0,1,0,0,0},
    {0,0,0,0,0,0,0,0},
    {0,1,0,0,0,0,0,0},
    {0,0,0,0,0,0,1,0},
    {0,0,0,0,0,0,0,0}
  };

  for(i=0; i<9; i++){
    for(j=0; j<8; j++){
      if (smiles[i][j]==1){
        r=i;
        c=j;
          base_servo.write(a[0][r][c]);
          shoulder_servo.write(a[1][r][c]);
          shoulder2_servo.write(a[2][r][c]);
          delay(500);
          elbow_servo.write(a[3][r][c]);
          delay(1500);
          base_servo.write(18);
          elbow_servo.write(180);
          shoulder_servo.write(60);
          shoulder2_servo.write(120);
          delay(500);
        }
      }
    }
  }


/*
 * Detaches all servos.
 */
void detachAll() {
  base_servo.detach();
  shoulder_servo.detach();
  shoulder2_servo.detach();
  elbow_servo.detach();
  delay(100);
}

/*
 * Attaches all servos.
 */
void attachAll() {
  base_servo.attach(BASE_SERVO_PIN);
  shoulder_servo.attach(SHOULDER_SERVO_PIN);
  shoulder2_servo.attach(SHOULDER2_SERVO_PIN);
  elbow_servo.attach(ELBOW_SERVO_PIN);
  delay(100);
}

void setup() {
  Serial.begin(9600);
  attachAll();
  Serial.println("---   START   ---");
}

void loop() {
  loopthrough(a);
  loopthrough(a);
  loopthrough(a);
  loopthrough(a);
}