/* ////////////////////////////////////////////////////////////////////////////
** File:      dsNavConConsole.pde
 VerCon[] = "dsNavConConsolle 0.1.0 by Guiott 02-08";
 Author:    Guido Ottaviani-->g.ottaviani@mediaprogetti.it<--
** Description: navigation control board remote consolle   
**
-------------------------------------------------------------------------------
Copyright 2008 Guido Ottaviani
g.ottaviani@mediaprogetti.it

    dsNavConConsole is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    dsNavConConsole is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with dsNavConConsole.  If not, see <http://www.gnu.org/licenses/>.
-------------------------------------------------------------------------------    
/////////////////////////////////////////////////////////////////////////////*/
import processing.serial.*;
import interfascia.*;

// input text fields
GUIController GuiRun;
IFTextField InputSpeed;
IFTextField InputDist;
IFTextField InputCoordX;
IFTextField InputCoordY;

GUIController GuiConfig;
IFTextField InputSpeed1Kp;
IFTextField InputSpeed1Ki;
IFTextField InputSpeed1Kd;
IFTextField InputSpeed2Kp;
IFTextField InputSpeed2Ki;
IFTextField InputSpeed2Kd;
IFTextField InputAngleKp;
IFTextField InputAngleKi;
IFTextField InputAngleKd;
IFTextField InputDistKp;
IFTextField InputDistKi;
IFTextField InputDistKd;
IFTextField InputAxle;
IFTextField InputKsp1;
IFTextField InputKsp2;
IFTextField InputKvel1;
IFTextField InputKvel2;
IFTextField InputRS232bps;
IFTextField InputRS232Com;

// buttons
ImageButtons BtnQuit;
ImageButtons BtnRS232;
ImageButtons BtnConfig;
ImageButtons BtnRun;
ImageButtons BtnSendSpeed;
ImageButtons BtnSendXY;
ImageButtons BtnSendDist;
ImageButtons BtnSendHalt;
ImageButtons BtnSendReset;
ImageButtons BtnSendVersion;

ImageButtons BtnSendCfgKSpeed1;
ImageButtons BtnSendCfgKSpeed2;
ImageButtons BtnSendCfgKAngle;
ImageButtons BtnSendCfgKDist;
ImageButtons BtnSendCfgWheels;
ImageButtons BtnSendCfgKvel;
ImageButtons BtnSendCfgRS232;
ImageButtons BtnSendCfgSave;
ImageButtons BtnSendCfgLoad;

// other images
PImage bg;
PImage bgConfig;
PImage Grid;
PImage LedRedOn;
PImage LedRedOff;
PImage LedGreenOn;
PImage LedGreenOff;

// gauges
float KnobAngle = -PI/2;
float KnobAngle1 = -PI/2;
int DesSpeed = 0;
int MesSpeed = 0;
float Current = 0;
float DesAngle = 0;
float DesAnglePrev = 0;
float MesAngle = 0;

boolean ConfigFlag = false;
boolean ReleasedFlag = false;

int Cycle = 0;
int CyclePeriod = 333;

int RelX = 432;
int RelY = 397;

int i = 0;
float PrevX = 0;
float PrevY = 0;
float DeltaX = 0;
float DeltaY = 0;
float Xpos = 0;
float Ypos = 0;
float Distance = 0;
float DeltaDist = 0;
float PrevXtmp = 0;      // just for simulation
float PrevYtmp = 0;      // just for simulation

int DesDist = 0;
int DesCoordX = 0;
int DesCoordY = 0;

int TxPeriod = 500;
int TxTimer;
boolean BlinkFlag;
boolean TxFlag = false;

Serial RS232Port;
int RS232ComPort = 99;
int RS232Bps;

String[] StrCfg = new String[19];  // config string
String[] SerialList = new String[0];  // declare an empty string for serial ports list 
byte[] TxBuff = new byte[32];  // Transmission buffer
int TxHeadLen = 4;             // TX header lenght
int[] TxIntValue = new int[8]; // int value buffer

int IdleCount = 0;
int IdleTime = 0;
int IdleTimeP = 0;

float CharTime = (0.2); // time period in ms for a char sent on RS232

byte[] RxBuff = new byte[32];

int Err = 0;
int HeadLen = 4;    // RX-TX header length

char Ver[] = new char[32];

// it will be set false after RS232 configuration to start real communication
boolean PreInitRS232Flag = true;  

/* in order to allow usage of the applet in a browser as a demo simulation,
   some functions that access to HW resources must me disabled, otherwise a
   security violation error occurs.
*/
boolean SimulationDrawFlag = true;
boolean SimulationCfgFlag = true;
boolean SimulationRS232Flag = true;

/*/////////////////////////////////////////////////////////////////////////////*/

void setup()
{
  size(1295, 815);
  smooth();
  
  PFont CharFont;
  CharFont = loadFont("HelveticaNeue-Bold-48.vlw"); 
  fill(255,255,255);
  textFont(CharFont, 15); 
  textAlign(CENTER);
  
  bg = loadImage("Panel.gif");
  bgConfig = loadImage("ConfigPanel.gif");
  background(bg);
  LedRedOn = loadImage("LedRedOn.gif");
  LedRedOff = loadImage("LedRedOff.gif");
  LedGreenOn = loadImage("LedGreenOn.gif");
  LedGreenOff = loadImage("LedGreenOff.gif");
  
  DefineButtons();
  
  strokeWeight(3.0);
  stroke(255,0,0);
 
  InputFieldsDefinition();
  
  LoadCfg();
  
  if (! SimulationRS232Flag)
  {
    i=0;
    while(i< Serial.list().length)
    {
      SerialList = (append(SerialList, Serial.list()[i]));
      i++;
    }
  }
  else
  {
    i=1;
    while(i < 5)
    {
      SerialList = (append(SerialList, "COM"+i));
      i++;
    }
    SerialList = (append(SerialList, "All COM ports are simulated"));
  }
}

// main loop --------------------------------------------------------------------
void draw()
{ 
  if (ConfigFlag)  // switch panels
  { 
    ButtonsCheckRun();
  }
  else
  {
    ButtonsCheckConfig();
  }
  
  IdleCount++;     // count idle period in ms
  if(IdleCount > 100)
  {
    IdleCount = 0;
    IdleTimeP = millis()-IdleTime;
    IdleTime=millis();
  }
  textAlign(LEFT);
  text(IdleTimeP/100, 1170, 760);
  textAlign(CENTER);
  text(Err, 1240, 760);
  
  if ((millis()-Cycle) > CyclePeriod)  // slow loop start------------------------
  {
    Cycle = millis();
    if (SimulationDrawFlag)          // simulation
    {
      SimulationDrawing();
    }
    else
    {
       Graph(Xpos,Ypos);
    }
    
    if (DesAnglePrev != DesAngle)
    {
      DesAnglePrev = DesAngle;
      TxIntValue[0] = (int)(DesAngle);
      TxData(0, 'O', 1, 1);
    }

    TxData(0, 'A', 0, 3);  // ask for all parameters
    Delay(10);             // wait for data to be received
    
    if (PreInitRS232Flag)
    {
      SimulationRxAll();      // simulation
    }
          
    if (RxData('A',10))
    {// two bytes -> i int
      MesSpeed = ((RxBuff[HeadLen] << 8) + (RxBuff[HeadLen+1]));
      Current = (float)((RxBuff[HeadLen+2] << 8) + (RxBuff[HeadLen+3]));
      Xpos = (float)((RxBuff[HeadLen+4] << 8) + (RxBuff[HeadLen+5]));
      Ypos = (float)((RxBuff[HeadLen+6] << 8) + (RxBuff[HeadLen+7]));
      MesAngle = (float)((RxBuff[HeadLen+8] << 8) + (RxBuff[HeadLen+9]));
      
      Current = abs(DesSpeed*2);                          // just for simulation
      MesSpeed = DesSpeed;                                // just for simulation 
      MesAngle = KnobAngle;                               // just for simulation                     
    }

    if (BlinkFlag)
    {
      BlinkFlag = false;
    }
    else 
    {
      BlinkFlag = true;
    }
  }                                    // slow loop end--------------------------
  
  LedBlink();
}
/*/////////////////////////////////////////////////////////////////////////////*/

/*-----------------------------------------------------------------------------*/
boolean RxData(char Cmd,int Len)
    {
      byte ChkSum = 0;
      
      if (! PreInitRS232Flag)  // not simulation
      {
        if (RS232Port.available() <= 0)
        {
          println("RX error: Timeout - No data received");
          Err++;
          return false;
        }
        else
        {
          for (i=0; i < Len+HeadLen+1; i++)  // loop for all data expected and only for that
          {
            RxBuff[i] = (byte)(RS232Port.read());
          }
        }
      }
    
      if (RxBuff[0] != '@')
      {
        println("RX error: Header not found. Expected: @  Found: " + (char)(RxBuff[0]));
        Err++;
        return false;
      }
      else if (RxBuff[2] != Cmd)
      {
        println("RX error: Wrong command. Expected: "+Cmd+" Found: " + (char)(RxBuff[2]));
        Err++;
        return false;
      }
      else if (RxBuff[3] != (Len+1))
      {
        println("RX error: Wrong length. Expected: "+ Len+" Found: " + (RxBuff[3]-1));
        Err++;
        return false;
      }
      
      for (i=HeadLen; i < Len+HeadLen; i++)  // loop only for data, header and ChkSum excluded
      {
        ChkSum += RxBuff[i];
      }
      
      if (RxBuff[i] != ChkSum)
      {
        println("RX error: CheckSum error. Expected: "+RxBuff[i] +" Found: " + ChkSum);
        Err++;
        return false;
      }
      else
      {
        return true;
      }
    }
    
/*-----------------------------------------------------------------------------*/
void Delay(int Bytes)
{
   /* wait for data to be received for a time (in ms) proportional to 
      num of bytes: data bytes + TX overhead + RX overhead (Bytes+5+5)
   */
   Bytes += 10;
   int Del= (int)((CharTime * Bytes) + 0.5);
   int StartDelay = millis();
   while ((millis()-StartDelay) < Del);
}

/*-----------------------------------------------------------------------------*/
void LedBlink()
{
  if (BlinkFlag)  // blink green (cycle) led
  {
   image(LedGreenOn,1170,700);
  }
  else
  {
   image(LedGreenOff,1170,700);
  }
    
  if (TxFlag)  // blink red (TX) led
  {
    image(LedRedOn,1230,700);
  }
  else
  {
    image(LedRedOff,1230,700);
    TxTimer = millis();
  }
  if ((millis() - TxTimer) >= TxPeriod)
  {
    TxTimer = millis();
    TxFlag = false;
  }
}

/*-----------------------------------------------------------------------------*/
void Graph(float Xcoord, float Ycoord)
{
    /* following statements are needed to translate the reference system from
    0(top-left), 750(bottom right) to -5000( bottom-left), +5000(top-right) mm
    */
    pushMatrix();
    translate(RelX,RelY);
    rotate(-HALF_PI);
    strokeWeight(3);
    stroke(255,0,0);
    Xcoord = Xcoord / 10000 * 750;
    Ycoord = Ycoord / 10000 * 750;
    // rotating system switches X with Y
    line(Ycoord, Xcoord, PrevY, PrevX);
    PrevX = Xcoord;
    PrevY = Ycoord;
    popMatrix();
}

/*-----------------------------------------------------------------------------*/  
int RangeCheckInt(String Data, int Min, int Max)
{
    int IntNum = 0;
    if (Data.length() != 0)
    {
      IntNum = Integer.parseInt(Data);
      if (IntNum < Min) IntNum = Min;
      if (IntNum > Max) IntNum = Max;
    }
    return IntNum;
}

/*-----------------------------------------------------------------------------*/  
float RangeCheckFloat(String Data, float Min, float Max)
{
    float FloatNum = 0;
    if (Data.length() != 0)
    {
      FloatNum = Float.parseFloat(Data);
      if (FloatNum < Min) FloatNum = Min;
      if (FloatNum > Max) FloatNum = Max;
      TxFlag = true;
    }
    return FloatNum;
}

/*-----------------------------------------------------------------------------*/  
void TxData(int Id, int Cmd, int ValueLen, int IntFlag)
{
/* Transmit a string toward dsNavCon board
   for a detailed description of protocol, see descrEng.txt in dsODO folder
*/
  int TxBuffCount;
  int ChkSum = 0;
  int CmdLen = 0;
  
  if (IntFlag == 1)  // integer value
  {
    CmdLen = ValueLen*2;          // 1 int value = 2 bytes to transmit
    for (TxBuffCount = 0; TxBuffCount < ValueLen; TxBuffCount++)
    {
      TxBuff[(TxBuffCount*2)+TxHeadLen] = (byte)(TxIntValue[TxBuffCount] >> 8);
      TxBuff[(TxBuffCount*2)+TxHeadLen+1] = (byte)(TxIntValue[TxBuffCount]);
    }
  }
  else if (IntFlag == 2); // long value
  {
    CmdLen = ValueLen*4;        // 1 long value = 4 bytes to transmit
    for (TxBuffCount = 0; TxBuffCount < ValueLen; TxBuffCount++)
    {
      TxBuff[(TxBuffCount*4)+TxHeadLen] = (byte)(TxIntValue[TxBuffCount] >> 24);
      TxBuff[(TxBuffCount*4)+TxHeadLen+1] = (byte)(TxIntValue[TxBuffCount] >> 16);
      TxBuff[(TxBuffCount*4)+TxHeadLen+2] = (byte)(TxIntValue[TxBuffCount] >> 8);
      TxBuff[(TxBuffCount*4)+TxHeadLen+3] = (byte)(TxIntValue[TxBuffCount]);
    }
  }
  
  TxBuff[0] = (byte)('@');
  TxBuff[1] = (byte)(Id);
  TxBuff[2] = (byte)(Cmd);
  TxBuff[3] = (byte)(CmdLen+1);  // included CheckSum

  for (i=0;i<(TxHeadLen+CmdLen);i++) 
  {
   ChkSum += TxBuff[i];
  }
  TxBuff[TxHeadLen+CmdLen] = (byte)(ChkSum);
  
  for (i=0;i<(TxHeadLen+CmdLen+1);i++) 
  {
    if (PreInitRS232Flag)
    {
      println(i + " " + TxBuff[i]);            // simulation
    }
    else
    {
      RS232Port.write(TxBuff[i]);
    }
  }
      
  if (IntFlag != 3) TxFlag = true; // avoid to blink TX led for continuos send
}

/*-----------------------------------------------------------------------------*/
void LoadCfg()  // Load data from config.txt file
{
  if (! SimulationCfgFlag)
  {
    if (loadStrings("config.txt") != null)
    {
      StrCfg = loadStrings("config.txt");
    }
    else 
    {
      CfgDefault();
      saveStrings("config.txt",StrCfg);
    }
  }
  else
  {
    CfgDefault();
  }
  FillTextFields();
}

/*-----------------------------------------------------------------------------*/
void CfgDefault()
{//if file config.txt is not present a new one is created with defaults
      StrCfg[0] = "6000";
      StrCfg[1] = "2000";
      StrCfg[2] = "200";
      StrCfg[3] = "6000";
      StrCfg[4] = "2000";
      StrCfg[5] = "200";
      StrCfg[6] = "9999";
      StrCfg[7] = "0";
      StrCfg[8] = "0";
      StrCfg[9] = "9999";
      StrCfg[10] = "0";
      StrCfg[11] = "0";
      StrCfg[12] = "1852222";
      StrCfg[13] = "5061455";
      StrCfg[14] = "5061455";
      StrCfg[15] = "9782453";
      StrCfg[16] = "9782453";
      StrCfg[17] = "57600";
      StrCfg[18] = "1";
}

/*-----------------------------------------------------------------------------*/
void FillTextFields()
{
       InputSpeed1Kp.setValue(StrCfg[0]);
       InputSpeed1Ki.setValue(StrCfg[1]);
       InputSpeed1Kd.setValue(StrCfg[2]);
       InputSpeed2Kp.setValue(StrCfg[3]);
       InputSpeed2Ki.setValue(StrCfg[4]);
       InputSpeed2Kd.setValue(StrCfg[5]);
       InputAngleKp.setValue(StrCfg[6]);
       InputAngleKi.setValue(StrCfg[7]);
       InputAngleKd.setValue(StrCfg[8]);
       InputDistKp.setValue(StrCfg[9]);
       InputDistKi.setValue(StrCfg[10]);
       InputDistKd.setValue(StrCfg[11]);
       InputAxle.setValue(StrCfg[12]);
       InputKsp1.setValue(StrCfg[13]);
       InputKsp2.setValue(StrCfg[14]);
       InputKvel1.setValue(StrCfg[15]);
       InputKvel2.setValue(StrCfg[16]);
       InputRS232bps.setValue(StrCfg[17]);
       InputRS232Com.setValue(StrCfg[18]);
 }
       
/*-----------------------------------------------------------------------------*/
void SaveCfg()  // Save current data to config.txt file
{
      StrCfg[0] = InputSpeed1Kp.getValue();
      StrCfg[1] = InputSpeed1Ki.getValue();
      StrCfg[2] = InputSpeed1Kd.getValue();
      StrCfg[3] = InputSpeed2Kp.getValue();
      StrCfg[4] = InputSpeed2Ki.getValue();
      StrCfg[5] = InputSpeed2Kd.getValue();
      StrCfg[6] = InputAngleKp.getValue();
      StrCfg[7] = InputAngleKi.getValue();
      StrCfg[8] = InputAngleKd.getValue();
      StrCfg[9] = InputDistKp.getValue();
      StrCfg[10] = InputDistKi.getValue();
      StrCfg[11] = InputDistKd.getValue();
      StrCfg[12] = InputAxle.getValue();
      StrCfg[13] = InputKsp1.getValue();
      StrCfg[14] = InputKsp2.getValue();
      StrCfg[15] = InputKvel1.getValue();
      StrCfg[16] = InputKvel2.getValue();
      StrCfg[17] = InputRS232bps.getValue();
      StrCfg[18] = InputRS232Com.getValue();
      if (! SimulationCfgFlag)
      {
        saveStrings("config.txt",StrCfg);
      }
}