#ifndef HOMEO2SERIAL_H
#define HOMEO2SERIAL_H

#define _POSIX_SOURCE 1 /* POSIX compliant source */                                      
#include <fcntl.h>
#include <termios.h>                                                                      
#include <unistd.h>                                                                       

#include "output.h"
#include "../microadam.h"
#include "../microeva.h"
#include "../../external/gnuplot/gnuplot.h"

extern "C"
{
  #include <opencv/highgui.h>
}

#define BAUDRATE B2400                                                                    
#define FALSE 0                                                                           
#define TRUE 1

namespace seemicro
{

class Homeo2Serial : public OutputModule
{
  Micro &micro;
  
  Gnuplot<std::string> gs;
  
  /**
    * Dieser Zeiger ermöglicht es des callback-Funktion, auf das
    * OutputObjekt zuzugreifen. home muß in jedem
    * ageleiteten OutputModule erneut abgeleitet werden
    * und läßt sich nicht in einer Klasse "CallbackUsingOutput"
    * zusammenfassen, da alle davon abgeleiteten Klassen
    * ja die static *home-Variable teilen würden.
    * OpenCV-Callbacks-Funktionen sollten einen solchen Pointer
    * als Argument geliefert bekommen!
    */
  static Homeo2Serial *home;
  
  /**
    * Saved settings of the serial port
    */
  struct termios oldtio;
  
  /**
    * device handle of serial port
    */
  int serialFd; 
  
  /**
    * Buffer for characters to be sent over serial device
    */
  char buf[255];

  /**
    * Winkel zur Zeit t und t+1 (vom Gyroskop bzw. von micro)
    */
  float wt, wt1;
  
  /**
    * Gewicht und Bias des Neurons, je -500 bis 500
    */
  float c, c0;

  /**
    * Lernparameter für Gewichte des Neurons, 1e-7 bis 1e3
    */
  float epsilon;
  
  /**
    * für die Trackbars
    */
  int iMittelAlpha, iEpsilon, ic, ic0, iGyroSkal;
  
  /**
    * um Werte zu berechnen, wie sie das Gyroskop liefert
    */
  float lastAlpha, deltaAlpha, deltaAlphaGemittelt;
  
  /**
    * -1000 bis 1000
    */
  float gyro_skal;

public:
  Homeo2Serial(Micro& Am, string serialDevice = "/dev/ttyS0") :
    micro(Am)
  {
    home = this;

    // init homeokinesis parameters
    c = 1.2;
    c0 = 0;
    epsilon = 0.1;
    gyro_skal = 2;
    setTrackbarVariables();

    gs.addChannel("deltaAlpha");
    gs.addChannel("deltaAlphaGemittelt");
    gs.addChannel("c");
    gs.addChannel("c0");
    gs.addChannel("gyro_skal");
    gs.addChannel("mittelAlphaKoeff");
    gs.addChannel("epsilon");
    gs.addChannel("toServo");

    cvNamedWindow("Homeo2Serial", 1);
    
    iMittelAlpha = 500;
    cvCreateTrackbar("Mittelwertkoeffizient für alpha", "Homeo2Serial",
      &iMittelAlpha, 1000, callback);
    cvCreateTrackbar("Epsilon", "Homeo2Serial", &iEpsilon, 1000, callback);
    cvCreateTrackbar("c", "Homeo2Serial", &ic, 1000, callback);
    cvCreateTrackbar("c0", "Homeo2Serial", &ic0, 1000, callback);
    cvCreateTrackbar("gyro_skal", "Homeo2Serial", &iGyroSkal, 2000, callback);
    
    struct termios newtio;                                  
                                                                                          
    /* open the device */
    if(DEBUGLEVEL>=2) cerr << "Opening serial device: '" << serialDevice << "'\n";
    serialFd = open(serialDevice.c_str(), O_RDWR | O_SYNC);                                                
    if(serialFd < 0)                                                                              
    {                                                                                       
      perror(serialDevice.c_str());                                                                  
      exit(-1);                                                                             
    }                                                                                       
                                                                                          
    /* save current port settings */                                                        
    tcgetattr(serialFd, &oldtio);                                                                  
                                                                                          
    /* set new port settings */                                                             
    newtio.c_cflag = BAUDRATE | CS8 | CLOCAL;                                               
    newtio.c_iflag = IGNPAR ;                                                               
    newtio.c_oflag = 0;                                                                     
    newtio.c_lflag = 0;                                                                     
    newtio.c_cc[VMIN]=1;                                                                    
    newtio.c_cc[VTIME]=0;                                                                   
    tcsetattr(serialFd, TCSANOW, &newtio);                                                          
    tcflush(serialFd, TCIFLUSH);
    
    cvResizeWindow("Homeo2Serial", 300, 400);    
  }                                                                                          

  ~Homeo2Serial()
  {
    cvDestroyWindow("Homeo2Serial");
    
    /* restore old port settings */                                                         
    if(DEBUGLEVEL>=2) cerr << "Resetting and closing serial port...";
    tcsetattr(serialFd, TCSANOW, &oldtio);   
    close(serialFd);
    if(DEBUGLEVEL>=2) cerr << endl;
  }

  void setTrackbarVariables()
  {
    iEpsilon = int( (log(epsilon)/log(10.)+7)*100 );
    ic = int(c);
    ic0 = int(c0);
    iGyroSkal = int(gyro_skal)+1000;
  }

  void putTrackbarVariables()
  {
    // set variables from trackbars
    epsilon = pow(10., iEpsilon*0.01-7);
    c = ic;
    c0 = ic0;
    gyro_skal = iGyroSkal-1000;
  }
                                                                                           
  float homeokinesis(float gyro_value)  
  {  
    static float tt = 0;

    float y = tanh(c*wt+c0) + 0.01 * sin(tt);// Output des Neurons
//    float sig = gyro_value*gyro_skal;                                                             
//    wt1 = y+sig;                                                                            
    c = c - 0.1*(2*y*y*c-1);                                                                

    float epsQ = epsilon*gyro_value*gyro_value+0.01;                                              
    c0 = c0-(epsQ*y);                                                                       

    wt = wt1;                                                                               
                                                                                          
    if (wt1>1) wt1=1;                                                                       
    if (wt1<-1) wt1=-1;                                                                     
                                                                                       
    tt++;                                                                                   
    return y;
  }                                                                                         
                    
  void output(bool keyframe)
  {
    deltaAlpha = micro.alpha - lastAlpha;
    lastAlpha = micro.alpha;
    if(deltaAlpha>=M_PI) deltaAlpha -= 2*M_PI;
    if(deltaAlpha<=-M_PI) deltaAlpha += 2*M_PI;

    if(iMittelAlpha==0) iMittelAlpha=1;
    deltaAlphaGemittelt += iMittelAlpha/1000.*(deltaAlpha-deltaAlphaGemittelt);
    float toServo = homeokinesis(deltaAlphaGemittelt);
    
    // serial output
    sprintf(buf, "Alpha=%f wt=%f\n", deltaAlphaGemittelt, toServo);
    if(DEBUGLEVEL>=3)
      printf("\rdeltaAlpha=% 1.6f deltaAlphaGemittelt=% 1.4f toServo=% 1.4f",
        deltaAlpha, deltaAlphaGemittelt, toServo);
    write(serialFd, buf, strlen(buf));         

    // gnuplot output
//    gs.putData("deltaAlpha", deltaAlpha);
    gs.putData("deltaAlphaGemittelt", deltaAlphaGemittelt);
//    gs.putData("c", c);
    gs.putData("c0", c0);
//    gs.putData("gyro_skal", gyro_skal);
//    gs.putData("mittelAlphaKoeff", iMittelAlpha/1000.);
//    gs.putData("epsilon", epsilon);
    gs.putData("toServo", toServo);    
    gs.plot();
    
    if(!keyframe) return;

    if(DEBUGLEVEL>=3) cerr << "Setting trackbars...";
    setTrackbarVariables();
    cvSetTrackbarPos("c", "Homeo2Serial", ic);
    cvSetTrackbarPos("c0", "Homeo2Serial", ic0);
    if(DEBUGLEVEL>=3) cerr << "OK\n";
  }

  static void callback(int dummy)
  {
    if(!home) return;
    float epsilonAlt = home->epsilon;
    float gyroSkalAlt = home->gyro_skal;
    home->putTrackbarVariables();
    if(DEBUGLEVEL>=1 && epsilonAlt != home->epsilon)
    {
      cout << "epsilon=" << home->epsilon << endl;
    }
    if(DEBUGLEVEL>=1 && gyroSkalAlt != home->gyro_skal)
    {
      cout << "gyro_skal=" << home->gyro_skal << endl;
    }
  }

}; // Homeo2Serial

} // seemicro

#endif
