#define NAXES 6 //Define the number of physical axes here
#define NAMELENGTH 8
#include <iostream>
#include <iomanip>

class Axis
{
    public:
    //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 limit 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.;

    //AXIS STATUS:
    double timestamp = 0.;
    int state_init = 0;
    int state_motion = 0;
    double pos = 0.;
    double v = 0.;
    double a = 0.;
 
    double targetpos = 0.;

    //Default Constructor:
    Axis();

    //Methods:
    int setName(const char * newname);
    int setUnitUsr(const char * newname);
    int setUnitCtr(const char * newname);
    int setStr(const char * setstring, char * targetstring);
    int print(void);
    int printh(void);
    int printl(void);
};

Axis::Axis() 
{
   
}

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

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

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

int Axis::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 Axis::print(void)
//
//
//
{
    std::cout << "=======================================" << std::endl;
    //Axis Parameters
    std::cout << "name ="     << name     << std::endl;   
    std::cout << "unit_usr =" << unit_usr << std::endl;  
    std::cout << "unit_ctr =" << unit_ctr << std::endl; 
    std::cout << "scale ="    << scale    << std::endl;
    std::cout << "offset ="   << offset   << std::endl; 
    //All limits in User Units:
    std::cout << "LimNeg ="      << LimNeg      << std::endl;
    std::cout << "LimPos ="      << LimPos      << std::endl;
    std::cout << "maxSpeed ="    << maxSpeed    << std::endl;
    std::cout << "minSpeed ="    << minSpeed    << std::endl; 
    std::cout << "maxJogSpeed =" << maxJogSpeed << std::endl; 
    std::cout << "minJogSpeed =" << minJogSpeed << std::endl;
 
    //Nominal speeds:
    std::cout << "moveSpeed =" << moveSpeed << std::endl; 
    std::cout << "jogSpeed  =" << jogSpeed  << std::endl;

    //AXIS STATUS:
    std::cout << "timestamp ="    << timestamp    << std::endl;
    std::cout << "state_init ="   << state_init   << std::endl;
    std::cout << "state_motion =" << state_motion << std::endl;
    std::cout << "pos ="          << pos          << std::endl;
    std::cout << "v ="            << v            << std::endl;
    std::cout << "a ="            << a            << std::endl;
 
    std::cout << "targetpos=" << targetpos << std::endl;
    return 0;
}


int Axis::printh(void)
// For Table Printing Print The Header.
{
    std::cout << "===================================" << std::endl;
    std::cout << std::left << std::internal;
     //Axis Parameters
    std::cout << "name     ";   
    std::cout << "unit_usr ";  
    std::cout << "unit_ctr "; 
    std::cout << "scale    ";
    std::cout << "offset   "; 
    //All limits 
    std::cout << "LimNeg      ";
    std::cout << "LimPos      ";
    std::cout << "maxSpeed    ";
    std::cout << "minSpeed    "; 
    std::cout << "maxJogSpeed "; 
    std::cout << "minJogSpeed ";
    //Nominal spe
    std::cout << "moveSpeed   "; 
    std::cout << "jogSpeed    ";

    //AXIS STATUS
    std::cout << "timestamp   ";
    std::cout << "state_init  ";
    std::cout << "state_motion";
    std::cout << "pos         ";
    std::cout << "v           ";
    std::cout << "a           ";
 
    std::cout << "targetpos   " << std::endl;
    return 0;
}

int Axis::printl(void)
// For Table Printing Print One Line.
{
    std::cout << std::left << std::internal;
    //Axis Parameters
    std::cout << std::left << std::setw(9) << name         ;   
    std::cout << std::left << std::setw(9) << unit_usr     ;  
    std::cout << std::left << std::setw(9) << unit_ctr     ; 
    std::cout << std::left << std::setw(9) << scale        ;
    std::cout << std::left << std::setw(9) << offset       ; 
    //All limits std::left << 12n User Units:
    std::cout << std::left << std::setw(12) << LimNeg       ;
    std::cout << std::left << std::setw(12) << LimPos       ;
    std::cout << std::left << std::setw(12) << maxSpeed     ;
    std::cout << std::left << std::setw(12) << minSpeed     ; 
    std::cout << std::left << std::setw(12) << maxJogSpeed  ; 
    std::cout << std::left << std::setw(12) << minJogSpeed  ;
    //Nominal spestd::left << 12ds:
    std::cout << std::left << std::setw(12) << moveSpeed    ; 
    std::cout << std::left << std::setw(12) << jogSpeed     ;

    //AXIS STATUSstd::left << 12
    std::cout << std::left << std::setw(12) << timestamp    ;
    std::cout << std::left << std::setw(12) << state_init   ;
    std::cout << std::left << std::setw(12) << state_motion ;
    std::cout << std::left << std::setw(12) << pos          ;
    std::cout << std::left << std::setw(12) << v            ;
    std::cout << std::left << std::setw(12) << a            ;
 
    std::cout << std::setw(12) << targetpos << std::endl;
    return 0;
}




////////////////////////////////////////////////////////////////
class NewGon
{
    public:
    //Create all Axis objects
    Axis axis[NAXES];
    
    //General Parameters
    char name[NAMELENGTH+1];
    int naxes = NAXES;

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

    //Constructors:
    NewGon() { }

    //Methods:
    int setName(const char * newname);
    int print(void);
};

//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::print(void)
//
{
    std::cout << "===================================" << std::endl;
    std::cout << "==== Contents of NewGon Object ====" << std::endl;
    std::cout << "General Parameters:" << std::endl;
    std::cout << "name[] = '" << name << "'" << std::endl;
    std::cout << "naxes = " << naxes << std::endl;
    std::cout << "===================================" << std::endl;
    std::cout << "General Status:" << std::endl;
    std::cout << "MODE = " <<  MODE << std::endl;
    std::cout << "timestamp = " << timestamp << std::endl;
    axis[0].printh();
    for (int i=0; i<naxes; i++)
    {
        axis[i].printl();
    }
    std::cout << "===================================" << std::endl;
    return 0;
}

