/*
 * walk.c
 *
 * Elliot Nicholls <nicho-ej@ee.uwa.edu.au>
 *
 * Program to allow the j5bot biped to walk with a preset gait
 *
 **********************************************************************/

#include <eyebot.h>
#include <stdio.h>
#include "gaitgen.h"
#include "trial.h"

#define NUM_LINKS 9
#define GAIT_TIME 3000
#define SMOOTH 20

typedef struct {
  int sacc[NUM_ACC];
  int frontAve;
  int front;
  int frontE;
  BYTE fsw;
  int rHipB;
  int lHipB;
} Sensors;

void gait(void);
void send(void);
void sendstr(char *);

ServoHandle servo[9];
int sIndex;
BYTE bmask[8] = {0x1, 0x2, 0x4, 0x8, 0x10, 0x20, 0x40, 0x80};
int accPort[NUM_ACC] = {7,15};
typedef enum{ frontTilt,sideTilt} ACCNAME;
Sensors sensors[GAIT_TIME*FPS/1000];
ACCDATA kP;


int main()
{ int i;
  char k;

  /* initialization */
  LCDPrintf("Here is Johnny..\n");
  sIndex = 0;
  kP[sideTilt] = 0;
  kP[frontTilt] = 4;

  /* get parameters */
  /* LCDMenu("+","-"," ","CON");*/

  /* allocate handels for all servos */
  servo[rHipT]=SERVOInit(RHipT);  servo[rHipB]=SERVOInit(RHipB);  
  servo[rKnee]=SERVOInit(RKnee);  servo[rAnkle]=SERVOInit(RAnkle);  
  servo[torso]=SERVOInit(Torso);  
  servo[lAnkle]=SERVOInit(LAnkle); servo[lKnee]=SERVOInit(LKnee);  
  servo[lHipB]=SERVOInit(LHipB);  servo[lHipT]=SERVOInit(LHipT);  

  /* middle position */
  LCDPrintf("Servos mid-pos..\n");
  for(i=0;i<NUM_LINKS;i++) SERVOSet(servo[i], states[0][i]);

  LCDMenu("GO","KP"," ","END");

  while ((k=KEYGet()) != KEY4)
  { LCDMenu("GO","KP"," ","END");
    switch (k) {
    case KEY2:
      LCDMenu("+","-"," ","END");
      while (k != KEY4){
	LCDPrintf("front: %d\n", kP[frontTilt]);
	k = KEYGet();
	if(k == KEY1) {
	  kP[frontTilt]++;
	} else if(k == KEY2) {
	  kP[frontTilt]--;
	}
      }
    }
  }
  gait();

  LCDMenu("REL"," "," ","END");
  k = KEYWait(KEY1);


  for(i = 0; i<9; i++) SERVORelease(servo[i]);  

  LCDMenu("SND"," "," ","END");

  while(KEYGet() == KEY1) {
    k = KEYWait(KEY1);
    send();
  }
  return 0;
}


void gait() {
  ANGLES new, angle;
  int stepLength = 1000/FPS;
  FANGLES step, fangle;
  int numSteps;
  int currFrame;
  int i, j;
  ACCDATA newAcc, contAcc, error, cumAcc, aveAcc;
  FACCDATA fAcc, stepAcc;

  for(i=0; i<NUM_ACC; i++) cumAcc[i] = 0;

  for(currFrame=0; currFrame<NUM_FRAMES-1; currFrame++) {
    numSteps = times[currFrame + 1] * FPS/1000;
    for(i=0; i<NUM_LINKS; i++) angle[i] = states[currFrame][i];
    for(i=0; i<NUM_LINKS; i++) new[i] = states[currFrame+1][i];
    for(i=0; i<NUM_ACC; i++) contAcc[i] = acc[currFrame][i];
    for(i=0; i<NUM_ACC; i++) newAcc[i] = acc[currFrame+1][i];
    for(i=0; i<NUM_LINKS; i++) step[i] = (float)(new[i] - angle[i]) / (float)numSteps;
    for(i=0; i<NUM_LINKS; i++) fangle[i] = (float)angle[i];
    for(i=0; i<NUM_ACC; i++) stepAcc[i] = (float)(newAcc[i] - contAcc[i]) / (float)numSteps;
    for(i=0; i<NUM_ACC; i++) fAcc[i] = (float)contAcc[i];

    for(j=0; j<numSteps; j++) {
      OSWait(stepLength/6);
      for(i=0; i<NUM_LINKS; i++) fangle[i] += step[i];
      for(i=0; i<NUM_ACC; i++) fAcc[i] += stepAcc[i];
      for(i=0; i<NUM_LINKS; i++) angle[i] = (int)fangle[i];
      for(i=0; i<NUM_ACC; i++) contAcc[i] = (int)fAcc[i];
      for(i=0; i<NUM_ACC; i++) sensors[sIndex].sacc[i] = OSGetAD(accPort[i]);

      sensors[sIndex].fsw = OSReadInLatch(0);

      for(i=0; i<NUM_ACC; i++) cumAcc[i] = cumAcc[i] + sensors[sIndex].sacc[i];
      if(sIndex > SMOOTH - 1) {
       for(i=0; i<NUM_ACC; i++) {
        cumAcc[i] = cumAcc[i] - sensors[sIndex-SMOOTH].sacc[i];
        aveAcc[i] = cumAcc[i]/SMOOTH;
       }
      }
      else {
       for(i=0; i<NUM_ACC; i++) {
         aveAcc[i] = (cumAcc[i])/(sIndex + 1);
       }
      }
      
      for(i=0; i<NUM_ACC; i++) error[i] = aveAcc[i] - contAcc[i];
      
      sensors[sIndex].frontAve = aveAcc[frontTilt];
      sensors[sIndex].front = contAcc[frontTilt];
      sensors[sIndex].frontE = error[frontTilt];

      /* correct torso angle */
      angle[torso] = angle[torso] + (kP[sideTilt]*error[sideTilt])/100;
      /* correct body attitude */
      angle[rHipB] = angle[rHipB] - (kP[frontTilt]*error[frontTilt])/100;
      angle[lHipB] = angle[lHipB] + (kP[frontTilt]*error[frontTilt])/100;
      sensors[sIndex].rHipB = angle[rHipB];
      sensors[sIndex].lHipB = angle[lHipB];
      
      sIndex++;
      for(i=0; i<NUM_LINKS; i++) SERVOSet(servo[i], angle[i]);                  
    }
  }

}

void send() {

  int maxIndex = sIndex;
  int i,j;
  char buff[50];
  char newline = '\n';
  char term = 4;
  
  sIndex = 0;

  OSInitRS232(SER57600, NONE, SERIAL1);
  for(i=0; i<maxIndex; i++) {
    for(j=0; j<NUM_ACC; j++) {
      sprintf(buff, "%d ", sensors[sIndex].sacc[j]);
      sendstr(buff);
    }
    for(j=0; j<8; j++) {
      sprintf(buff, "%d ", (int)(0x1 ^ (sensors[sIndex].fsw & bmask[j])/bmask[j]));
      sendstr(buff);
    }
    sprintf(buff, "%d ", sensors[sIndex].front);
    sendstr(buff);
    sprintf(buff, "%d ", sensors[sIndex].frontAve);
    sendstr(buff);
    sprintf(buff, "%d ", sensors[sIndex].frontE);
    sendstr(buff);
    sprintf(buff, "%d ", sensors[sIndex].rHipB);
    sendstr(buff);
    sprintf(buff, "%d ", sensors[sIndex].lHipB);
    sendstr(buff);
    OSSendRS232(&newline, SERIAL1);
    sIndex++;
  }
  OSSendRS232(&term, SERIAL1);
}

void sendstr(char s[]) {
  int i = 0;

  while (s[i] != 0) {
    OSSendRS232(&s[i], SERIAL1);
    LCDPrintf("%c",s[i]);
    i++;
  }
}