/* 
   *  Sonar sensors cannot be used when using the serial interface!
   *  Build for Duemilanove with 328. 
   *  
   *  Changes to version 30-10-2018: 
   *  - The Sonar code has been completely stripped out because the serial port is not available for the Sonars.
   *  - Brackets has been placed in the two code lines for setting the PWM frequency, because it was causing compiler warnings.
   *  - The function allStop has been modified according the solution provided by asantiago@bnl.gov
   */

template<class T> inline Print &operator <<(Print &obj, T arg) { obj.print(arg); return obj; }

#include <MotorWheel.h>
#include <Omni4WD.h>
#include <PID_Beta6.h>
#include <PinChangeInt.h>
#include <PinChangeIntConfig.h>
//#include <SONAR.h>

/*
************************************************************************************
                                     Sonar:0x12
                           ------------------------------
                          |                             |
                        M3|                             |M2
                          |                             |
                          |                             |
                          |                             |
                Sonar:0x13|                             |Sonar:0x11
                          |                             |
                          |                             |Power Switch
                          |                             |
                          |                             |
                          -------------------------------
                          |                             |
                        M4|                             |M1
                          |                             |
                          -------------------------------
                                    Sonar:0x14                          
************************************************************************************
*/


int speedRampTime = 1000;  //speed ramping time in ms.

irqISR(irq1,isr1);
MotorWheel wheel1(3,2,4,5,&irq1);

irqISR(irq2,isr2);
MotorWheel wheel2(11,12,14,15,&irq2);

irqISR(irq3,isr3);
MotorWheel wheel3(9,8,16,17,&irq3);

irqISR(irq4,isr4);
MotorWheel wheel4(10,7,18,19,&irq4);

Omni4WD Omni(&wheel1,&wheel2,&wheel3,&wheel4);



// er zitten delays in deze functies.
void goAhead(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_ADVANCE) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarAdvance(0);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void turnLeft(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_LEFT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarLeft(0);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void turnRight(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_RIGHT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarRight(0);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void rotateRight(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_ROTATERIGHT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarRotateRight(0);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void rotateLeft(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_ROTATELEFT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarRotateLeft(0);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void allStop(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_STOP) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarStop();
        //this sets the PWM pins Low to enforce a stop. Solution from asantiago@bnl.gov via email conversation.
        digitalWrite(3, LOW);digitalWrite(11, LOW);
        digitalWrite(9, LOW);digitalWrite(10, LOW);
}

void backOff(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_BACKOFF) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarBackoff(0);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void upperLeft(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_UPPERLEFT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarUpperLeft(speedMMPS);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void lowerLeft(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_LOWERLEFT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarLowerLeft(speedMMPS);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void upperRight(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_UPPERRIGHT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarUpperRight(speedMMPS);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

void lowerRight(int speedMMPS){
    if(Omni.getCarStat()!=Omni4WD::STAT_LOWERRIGHT) Omni.setCarSlow2Stop(speedRampTime);
        Omni.setCarLowerRight(speedMMPS);
        Omni.setCarSpeedMMPS(speedMMPS, speedRampTime);
}

/*
   backoff   = achteruit
   turnLeft  = opzij links
   upperLeft = diagonaal links naar voren
*/
       
void(*function[11])(int speedMMPS) =
      {goAhead, turnLeft, turnRight, rotateRight, rotateLeft, allStop,
       backOff, upperLeft, lowerLeft, upperRight, lowerRight};


#define NULLCHAR   0
#define SPACE     32
#define ESC       27
#define BACKSPACE  8
#define TAB        9
#define LF        10
#define CR        13

// Last active time
unsigned long int lastactime, currMillis, debtime;

char autoshutdown = 0;
char autoreply = 0;
char repeatmode = 0;

char sendpacket[24];
char error_code = 0;

int speedMMPS = 0;

void setup() {
  delay(2000);  
  TCCR1B = (TCCR1B & 0xf8) | 0x01;    // Pin9,Pin10 PWM 31250Hz
  TCCR2B = (TCCR2B & 0xf8) | 0x01;    // Pin3,Pin11 PWM 31250Hz
    
  //SONAR::init(13);    
  //Omni.switchMotors();    
  Omni.PIDEnable(0.35,0.02,0,10);

  Serial.begin(28800);     // opens serial port, sets data rate to 28800 bps.
                           // Faster does not work with all motor running fast.
  lastactime = currMillis = millis();

  sendpacket[0] = 2; // STX
  sendpacket[23] = 3;// ETX
}

void sendinfo(void) {
  int i;

  sendpacket[1] = 1; // info packet
  sendpacket[2] = 1; // version
  sendpacket[3] = 1;
  sendpacket[4] = error_code;
  sendpacket[5] = Omni.getCarStat();
  sendpacket[6] = (speedMMPS     ) & 0xff ;
  sendpacket[7] = (speedMMPS >> 8) & 0xff;

  Serial.write(sendpacket, 24);

  /* getCarStat --> info
  Serial.print("Info: ");
  Serial.print(" ");
  Serial.print((char)Omni.getCarStat());
  Serial.print(" ");
  Serial.print(distBuf[0]);
  Serial.print(" ");
  Serial.print(distBuf[0]);
  Serial.print(" ");
  Serial.print(distBuf[0]);
  Serial.print(" ");
  Serial.print(distBuf[0]);
  Serial.println();
  */
}



// number of received chars from next packet
unsigned char nmbr = 0; 
unsigned char packet[16];
char dosendinfo = 0;

void loop() {
  char c;
  //unsigned char sonarcurrent = 0;
  short frontleft, backleft, frontright, backright;

  // try to complete getting a packet and then process it.
  if (Serial.available() > 0) {
    c = Serial.read();
    packet[nmbr++] = c;
    if (nmbr > 15) { // process next packet
      if (packet[0] != 2 ||  packet[15] != 3) {
        // set error in info packet
        error_code = 1;
        //Serial.println("Packet framing error");
      }
      /*      Serial.println((int)packet[0]);
      Serial.println((int)packet[1]);
      Serial.println((int)packet[2]);
      Serial.println((int)packet[3]);
      Serial.println((int)packet[4]);
      Serial.println((int)packet[5]);
      Serial.println((int)packet[6]);
      Serial.println((int)packet[7]);
      Serial.println();

      Serial.println("Process Command");
      */
      if (packet[1] < 11) {
        speedMMPS = (int)((unsigned int)packet[2]+((unsigned int)packet[3])*256);
        (*function[packet[1]])(speedMMPS);
        //Serial.println(packet[1]);
        //Serial.println(speedMMPS);
       }
       else {
        switch (packet[1]) {
          case 12: // set speed of all four motors
          //debtime = millis(); 
          frontleft  = (short)((unsigned short)packet[2]+((unsigned short)packet[3])*256);
          backleft   = (short)((unsigned short)packet[4]+((unsigned short)packet[5])*256);
          frontright = (short)((unsigned short)packet[6]+((unsigned short)packet[7])*256);
          backright  = (short)((unsigned short)packet[8]+((unsigned short)packet[9])*256);
          if (frontleft > 700) frontleft = 700;
          else if (frontleft < -700) frontleft = -700;
          if (frontright > 700) frontright = 700;
          else if (frontright < -700) frontright = -700;
          if (backleft > 700) backleft = 700;
          else if (backleft < -700) backleft = -700;
          if (backright > 700) backright = 700;
          else if (backright < -700) backright = -700;

          if ( (frontleft==0) && (backleft==0) ) {
            allStop(0);
          }
          else {
            
            Omni.wheelULSetSpeedMMPS(frontleft);
            Omni.wheelLLSetSpeedMMPS(backleft);
            Omni.wheelLRSetSpeedMMPS(-frontright);
            Omni.wheelURSetSpeedMMPS(-backright);
          }

          /*
          debtime = millis()- debtime; 
          debtime = (short int) debtime;
          sendpacket[10] = (debtime     ) & 0xff ;
          sendpacket[11] = (debtime >> 8) & 0xff;
          sendinfo();
          */
        break;
        case 13: // set speedMMPS
          speedMMPS = (int)((unsigned short)packet[2]+((unsigned short)packet[3])*256);
          if (speedMMPS > 700) speedMMPS = 700;
          else if (speedMMPS < -700) speedMMPS = -700;
          break;
          case 20: // ask info packet
            dosendinfo = 1;
          break;
          case 21:
            autoshutdown = 0;
          break;
          case 22:
            autoshutdown = 1;
          break;
          case 23:
            autoreply = 0;
          break;
          case 24:
            autoreply = 1;
          break;
          case 25:
            error_code = 2;
            //Serial.println("25 Not implemented");
          break;
          case 26:
            error_code = 3;
            //Serial.println("Not implemented");
          break;
          case 27: // keep alive 
          break;
          case 28:
            repeatmode = 0;
          break;
          case 29:
            repeatmode = 1;
          break;
          default:
            error_code = 3;
            //Serial.println("Unrecognized command");
          break;
          }
        }

        if (autoreply || dosendinfo) sendinfo();
        dosendinfo = 0;

        // clear packet buffer
        nmbr = 0;
        lastactime = millis(); 
      }
  }


  if (autoshutdown) {
    if ((millis() - lastactime) > 3000) {
      //Serial.print("Connection lost?");
      //Serial.print(millis());
      //Serial.println();
      allStop(0);
      lastactime = millis();
    }
  }
  Omni.PIDRegulate();
}