#define GRIPPER "NXT SM1"
#define LIFTER "NXT SM1"

#define AXLE_NS "NXT SM2"
#define AXLE_EW "NXT SM1"

#define MOTOR_N OUT_A
#define MOTOR_S OUT_B
#define MOTOR_E OUT_A
#define MOTOR_W OUT_B

#define MOTOR_N_ROTDIR -1
#define MOTOR_S_ROTDIR 1
#define MOTOR_E_ROTDIR -1
#define MOTOR_W_ROTDIR 1

task main() {
   string nxtname;
   string message, submsg, command;
   bool startok, endok;
   int a,b,c;
   string sp0, sp1, sp2;
   string axle, type;
   float param1, param2;
   byte motor1, motor2;
   long angel1, angel2;
   
   byte gripperdir;
   long liftercount;

   long waitopen, waitclose, waitturn, waitdoubleturn;


   SetSensorLowspeed(S1);
   nxtname =  LeftStr(BrickDataName(),7);

   SetMotorRegulationTime(MS_10);
   SetMotorRegulationOptions(OUT_REGOPTION_NO_SATURATION);
   PosRegSetMax(OUT_A, 0, 0);
   PosRegSetMax(OUT_B, 0, 0);
   PosRegEnable(OUT_A);
   PosRegEnable(OUT_B);

   TextOut(2, LCD_LINE1, "NXT Cube Machine", false);
   TextOut(2, LCD_LINE2, "****************", false);
   TextOut(9, LCD_LINE3, "Start  " + nxtname, false);
   TextOut(2, LCD_LINE4, "----------------", false);

   PlaySound(SOUND_CLICK);

   byte rt = MotorRegulationTime();
   byte ro = MotorRegulationOptions();
   byte ms = MotorMaxSpeed(OUT_A);
   byte ma = MotorMaxAcceleration(OUT_A);
   
   waitopen = 265;
   waitclose = 265;
   waitturn = 500;
   waitdoubleturn = 1000;

   do {
      do {
         Wait(50);
      } while (ReceiveMessage(MAILBOX1,true,message) == STAT_MSG_EMPTY_MAILBOX)

      a = Pos("(",message);
      c = Pos(")",message);
      if (a > 0 && c > 0) {
         command = SubStr(message, 0 ,a);
         submsg = SubStr(message, a+1, c-a-1);
         startok = true;
      } else { startok = false; }

      TextOut(2, LCD_LINE5, "                ", false);
      TextOut(2, LCD_LINE5, command, false);
      TextOut(2, LCD_LINE6, "                ", false);
      TextOut(2, LCD_LINE6, submsg, false);
      TextOut(2, LCD_LINE7, "----------------", false);
      TextOut(2, LCD_LINE8, "                ", false);

      if (startok) {
         sp0 = ""; sp1 = "0"; sp1 = "0"; sp2 = "0";
         if (Pos(",",submsg) > 0) {
            b = Pos(",",submsg); sp0 = SubStr(submsg, 0, b); submsg = SubStr(submsg, b+1, StrLen(submsg) - b - 1);
         } else { sp0 = submsg; }
         if (Pos(",",submsg) > 0) {
            b = Pos(",",submsg); sp1 = SubStr(submsg, 0, b); submsg = SubStr(submsg, b+1, StrLen(submsg) - b - 1);
         } else { sp1 = submsg; }
         if (Pos(",",submsg) > 0) {
            b = Pos(",",submsg); sp2 = SubStr(submsg, 0, b); submsg = SubStr(submsg, b+1, StrLen(submsg) - b - 1);
         } else { sp2 = submsg; }
      }

      if (startok) {
         if (command == "SETWAIT") {
            type = sp0;
            param1 = StrToNum(sp1);
            if (type == "OPEN") { waitopen = param1; }
            if (type == "CLOSE") { waitclose = param1; }
            if (type == "TURN") { waitturn = param1; }
            if (type == "DOUBLETURN") { waitdoubleturn = param1; }
            endok = true;
         }

         if (command == "TURN") {
            axle = sp0;
            param1 = StrToNum(sp1);
            param2 = StrToNum(sp2);
            if (axle == "NS" && AXLE_NS == nxtname || axle == "EW" && AXLE_EW == nxtname) {
               if (axle == "NS") {
                  motor1 = MOTOR_N;
                  angel1 = 210 * param1 * MOTOR_N_ROTDIR;
                  motor2 = MOTOR_S;
                  angel2 = 210 * param2 * MOTOR_S_ROTDIR;
               }
               if(axle == "EW") {
                  motor1 = MOTOR_E;
                  angel1 = 210 * param1 * MOTOR_E_ROTDIR;
                  motor2 = MOTOR_W;
                  angel2 = 210 * param2 * MOTOR_W_ROTDIR;
               }
               PosRegAddAngle(motor1, angel1);
               PosRegAddAngle(motor2, angel2);
               if (abs(angel1) || abs(angel2) > 210) { Wait(waitdoubleturn); } else { Wait(waitturn); }
               endok = true;
            }
         }

         if (command == "OPEN" || command == "CLOSE") {
            axle = sp0;
            param1 = StrToNum(sp1);
            param2 = StrToNum(sp2);
            if (GRIPPER == nxtname) {
               if (command == "OPEN") { gripperdir = PF_CMD_FWD; } else { gripperdir = PF_CMD_REV; }
               if (axle == "NS" && param1 == 1) { HTPFComboDirect(S1, PF_CHANNEL_1, PF_CMD_BRAKE, gripperdir); }
               if (axle == "NS" && param2 == 1) { HTPFComboDirect(S1, PF_CHANNEL_2 ,PF_CMD_BRAKE, gripperdir); }
               if (axle == "EW" && param1 == 1) { HTPFComboDirect(S1, PF_CHANNEL_1, gripperdir, PF_CMD_BRAKE); }
               if (axle == "EW" && param2 == 1) { HTPFComboDirect(S1, PF_CHANNEL_2, gripperdir, PF_CMD_BRAKE); }
               if (command == "OPEN") { Wait(waitopen); } else { Wait(waitclose); }
               HTPFComboDirect(S1, PF_CHANNEL_1, PF_CMD_BRAKE, PF_CMD_BRAKE);
               HTPFComboDirect(S1, PF_CHANNEL_2, PF_CMD_BRAKE, PF_CMD_BRAKE);
               endok = true;
            } else { endok = true; }
         }

         if (LIFTER == nxtname && command == "LIFT") {
            param1 = StrToNum(sp0);
            RotateMotor(OUT_C, 100, 360 * param1);
            endok = true;
         }

         if (LIFTER == nxtname && command == "LIFTERINIT") {
            Off(OUT_C);
            ResetRotationCount(OUT_C);
            Wait(1);
            do {
               liftercount = MotorRotationCount(OUT_C);
               OnRev(OUT_C, 40);
               Wait(500);
               Coast(OUT_C);
            } while (abs(MotorRotationCount(OUT_C) - liftercount) > 100)
            RotateMotor(OUT_C, 100, 180);
            endok = true;
         }
         
         if (command == "ALERT") {
            param1 = StrToNum(sp1);
            param2 = StrToNum(sp2);
            if (param1 == 1) { PlaySound(SOUND_CLICK); }
            if (param2 > 0) { Wait(param2); }
            endok = true;
         }
         
         if (command == "WAIT") {
            param1 = StrToNum(sp0);
            Wait(param1);
            endok = true;
         }

         if (endok) {
            TextOut(2,LCD_LINE8, "OK", false);
            SendRemoteString(0, MAILBOX1, "OK");
            endok = false;
         }
      }
   } while(true)
}