#include <math.h>

#include "ros/ros.h"
#include "command/setvalue.h"
#include "command/getvalue.h"
#include "command/newgon_state.h"

///////////////////////////////////////////////////////////////////////////////
class AxisDef
{
    public:
    static const int NAMELENGTH = 8;
    //AXIS PARAMETERS
    char name[NAMELENGTH+1];       //axis name
    char unit_usr[NAMELENGTH+1];   //user unit name (string)
    char unit_ctr[NAMELENGTH+1];   //control unit name (string)
    double scale = 1.;    //
    double offset = 0.;   //UserUnits = (ControlUnits * scale) + offset
   //All limits in User Units:
    double LimNeg = 0.;
    double LimPos = 0.;
    double maxSpeed = 0.;
    double minSpeed = 0.;
    double maxJogSpeed = 0.;
    double minJogSpeed = 0.;
    //Nominal speeds:
    double moveSpeed = 0.;
    double jogSpeed = 0.;
    //Motion paramters 
    double dt = 0.001;   //time step
    double vmax = 1.;     //maximum velocity
    double amax = 1.;     //maximum acceleration
    double xdb = 0.0005; //position loop deadband.
 
    //AXIS STATUS:
    int state_init = 0;
    int state_motion = 0;

    double t = 0;  //current time
    double x = 0;  //current position
    double v = 0;  //current velocity
    double a = 0;  //current acceleration

    double xT = 0; //target position

    //Default Constructor:
    AxisDef();

    //Methods:
    int setName(const char * newname);
    int setUnitUsr(const char * newname);
    int setUnitCtr(const char * newname);
    int setStr(const char * setstring, char * targetstring);
    int set_target(double target);
    int step();
};

AxisDef::AxisDef() 
{
    //initialize strings to "" (to avoid crap upon creation)
    setName("");
    setUnitUsr("");
    setUnitCtr(""); 
}

int AxisDef::setName(const char * newname)
{
    return setStr(newname, name);
}

int AxisDef::setUnitUsr(const char * newname)
{
    return setStr(newname, unit_usr);
}

int AxisDef::setUnitCtr(const char * newname)
{
    return setStr(newname, unit_ctr);
}

int AxisDef::setStr(const char * setstring, char * targetstring)
// use: newgon.setName("Name")
// only the first NAMELENTH charaters will be used.
// if the given string is longer than NAMELENGTH, return 1
// if all goes well return 0
{
    for (int i=0; i<NAMELENGTH+1; i++) 
    {
        if (setstring[i] == 0) //if end of string of newname
        {
            targetstring[i]=setstring[i];
            break;
        }
        else { //normal characters in newname
            targetstring[i]=setstring[i];
        }
	if (i==NAMELENGTH) //if the end of name is reached, terminate with 0
        {
            targetstring[i]=0;
            return 1; // return 1 means: string too long.
        }          
    }
    return 0;
}

int AxisDef::set_target(double target)
{
    xT = target;
    return 0;
}

int AxisDef::step()
{
    t += dt; //increment by time step
    double xrem = xT-x; //remaining path
    if (fabs(xrem) < xdb) return 1; //if in deadband, don't do anythhing

    double xsgn = xrem<0?-1:1; //sgn(xrem)
    double bremsweg = v*v/2/amax; //bremsweg (based on current speed)
    double asgn = fabs(xrem)<fabs(bremsweg)?-1:1; //if bremsweg too short deccelerate
    a = xsgn*asgn*amax;  //calculate new a
    v = v + a*dt;        //calculate new v
    if (v> vmax) {v =  vmax; a=0;}  //bound v
    if (v<-vmax) {v = -vmax; a=0;}
    x = x + v*dt + 0.5*a*dt*dt; //calculate new x
    return 1;
}


////////////////////////////////////////////////////////////////////////////////
class NewGon
{
    public:
    static const int naxes = 6;       //define here the number of axes of NewGon
    static const int namelength = 20;  //define here the max string length of names
    
    //NOTE: AXIS DEFINITIONS START AT 1 not 0, therefore axis[0] is empty and not used.

    //Create all Axis objects
    AxisDef axis[naxes+1];

    //General Parameters
    char name[namelength+1];
    double dt = 0.001;

    //GENERAL STATUS
    int MODE = 0;
    double timestamp = 0.;

    //Constructors:
    NewGon();

    //Methods:
    int setName(const char * newname);
    int setRate(double frequency);
    int step();
    bool setValueROS(command::setvalue::Request& req,
                     command::setvalue::Response& res);
    bool getValueROS(command::getvalue::Request& req,
                     command::getvalue::Response& res);
    int publishROS(ros::Publisher * pPub);
};

NewGon::NewGon()
{

}

int NewGon::setName(const char * newname)
// use: newgon.setName("Name")
// only the first NAMELENTH charaters will be used.
// if the given string is longer than NAMELENGTH, return 1
// if all goes well return 0
{
    for (int i=0; i<namelength+1; i++) 
    {
        if (newname[i] == 0) //if end of string of newname
        {
            name[i]=newname[i];
            break;
        }
        else { //normal characters in newname
            name[i]=newname[i];
        }
	if (i==namelength) //if the end of name is reached, terminate with 0
        {
            name[i]=0;
            return 1; // return 1 means: string too long.
        }          
    }
    return 0;
}

int NewGon::setRate(double frequency)
{
    dt = 1./frequency;
    //Watch out frequency should be < 0
    return 0;
}

int NewGon::step ()
{
    for (int i=1; i<naxes+1; i++) 
    {
         axis[i].dt = dt;
         axis[i].step();
    }
    return 0;
}



//SET COMMAND LIST:
bool NewGon::setValueROS(command::setvalue::Request  &req,
                         command::setvalue::Response &res)
{
    ROS_INFO("setvalue request : %s: %lf", req.axis.c_str(), req.value);
    if (!req.axis.compare("s1"))
    {
        axis[1].xT = req.value;
        res.response = "Axis 1: " + std::to_string(req.value);
    }
    else if (!req.axis.compare("s2"))
    {
        axis[2].xT = req.value;
        res.response = "Axis 2: " + std::to_string(req.value);
    }
    else if (!req.axis.compare("s3"))
    {
        axis[3].xT = req.value;
        res.response = "Axis 3: " + std::to_string(req.value);
    }
    else if (!req.axis.compare("s4"))
    {
        axis[4].xT = req.value;
        res.response = "Axis 4: " + std::to_string(req.value);
    }
    else if (!req.axis.compare("s5"))
    {
        axis[4].xT = req.value;
        res.response = "Axis 5: " + std::to_string(req.value);
    }
    else if (!req.axis.compare("MODE"))
    {
        MODE = (int)req.value;
        res.response = "MODE: " + std::to_string(req.value);
    }


   ROS_INFO("setvalue response: %s", res.response.c_str());
    return true;
}

bool NewGon::getValueROS(command::getvalue::Request  &req,
                         command::getvalue::Response &res)
{
    ROS_INFO("getvalue request : %s", req.axis.c_str());
    res.response = "ok.";
    ROS_INFO("getvalue response: %s", res.response.c_str());
    return true;
}

int NewGon::publishROS(ros::Publisher * pPub)
{
    command::newgon_state msg;
    msg.s1 = axis[1].x;
    msg.s2 = axis[2].x;
    msg.s3 = axis[3].x;
    msg.s4 = axis[4].x;
    msg.s5 = axis[5].x;

    pPub->publish(msg);
}


