#include "macs.h"

#include <iostream.h>
#include <sys/time.h>   // timeval->tv_sec

#include "timer.h"
#include "network.h"

Macs::Macs(BYTE_t nodeid, ostream &out=cout)
    : NodeDrv(nodeid, out), fMacId(2*nodeid+1),
    fPos(0), fPosTime(0.0), fPdoPos(0), fPdoTime(0.0),
    fPosActive(0), fRpmActive(0), fError(FALSE)
{
}

Macs::~Macs()
{
}

void Macs::HandleSDO(WORD_t idx, BYTE_t subidx, LWORD_t val, struct timeval *tv)
{
    switch (idx)
    {
    case 0x100a:
        lout << "- Mac using Software Version V" << dec << (int)(val>>16) << "." << (int)(val&0xff) << endl;
        return;

    case 0x2002:
        cout << "Actual velocity: " << dec << val << endl;
        fVel = val;
        return;

    case 0x6004:
        if (subidx)
            return;

//        lout << "Actual Position: " << dec << (signed long)val << endl;
        fPos = (LWORDS_t)val;
        fPosTime.SetTimer(tv);
        return;
/*
    case 0x2001:
        cout << "Axe Status: 0x" << hex << val << endl;
        cout << " - Motor "             << (val&0x01?"standing":"moving") << endl;
        cout << " - Positioning "       << (val&0x02?"active":"inactive") << endl;
        cout << " - Rotary mode "       << (val&0x04?"active":"inactive") << endl;
        cout << " - Attitude control: " << (val&0x40?"off":"on") << endl;
        cout << " - Axe resetted: "     << (val&0x80?"yes":"no") << endl;
        fPosActive = val&0x02;
        fRpmActive = val&0x04;
        return;
    case 0x2003:
        if (!subidx)
        {
            cout << "Input State: ";
            for (int i=0; i<8; i++)
                cout << (int)(val&(1<<i)?1:0);
            cout <<endl;
            return;
        }
        cout << "Input No." << subidx << (val?"hi":"lo") << endl;
        return;
    case 0x2004:
        cout << "Status Value of Axis: 0x" << hex << val << endl;
        cout << " - Attitude control: "        << (val&0x800000?"off":"on") << endl;
        cout << " - Movement done: "           << (val&0x040000?"yes":"no") << endl;
        cout << " - Number of Control Units: " << (int)((val>>8)&0xff) << endl;
        cout << " - Attitude control: "        << (val&0x04?"off":"on") << endl;
        cout << " - Startswitch active: "      << (val&0x20?"yes":"no") << endl;
        cout << " - Referenceswitch active: "  << (val&0x40?"yes":"no") << endl;
        cout << " - Endswitch active: "        << (val&0x80?"yes":"no") << endl;
        return;
*/

    case 0x6002:
        lout << "- Velocity resolution #" << (int)GetId() << ": " << dec << val << " ticks/min" << endl;
        fVelRes = val;
        return;
    }
    cout << "Macs: SDO, idx=0x"<< hex << idx << "/" << (int)subidx;
    cout << ", val=0x"<<val<<endl;
}

void Macs::ReqVelRes()
{
    lout << "- Requesting velocity resolution (velres, 0x3007) of " << (int)GetId() << endl;
    RequestSDO(0x6002);
    WaitForSdo(0x6002);
}

void Macs::SetPDO1On(BYTE_t flag)
{
    lout << "- " << (flag?"Enable":"Disable") << " PDO1 of #" << (int)GetId() << endl;
    SendSDO(0x1800, 1, (LWORD_t)(flag?0:1)<<31); 
    WaitForSdo(0x1800, 1);           
}

void Macs::InitDevice(Network *net)
{
    NodeDrv::InitDevice(net);

//    SendSDO(0x4003, (LWORD_t)('E'<<24 | 'X'<<16 | 'I'<<8 'T'));
//    WaitForSdo(0x4003, 0);

/*
    lout << "- Requesting SDO 0x2002 (vel) of " << (int)GetId() << endl;
    RequestSDO(0x2002);
    WaitForSdo(0x2002);

    lout << "- Requesting SDO 0x2003 of " << (int)GetId() << endl;
    RequestSDO(0x2003);
    WaitForSdo(0x2003);

    lout << "- Requesting SDO 0x2004 of " << (int)GetId() << endl;
    RequestSDO(0x2004);
    WaitForSdo(0x2004);


*/
    lout << "- Requesting Mac Software Version of " << (int)GetId() << endl;
    RequestSDO(0x100a);
    WaitForSdo(0x100a);
    //    
    SetRpmMode(FALSE);

    ReqVelRes(); // Init fVelRes

    lout << "- Motor on of " << (int)GetId() << endl;
    SendSDO(0x3000, string('O', 'N'));
    WaitForSdo(0x3000);


    SetHome(250000);

//    lout << "- Requesting SDO 0x2001 of " << (int)GetId() << endl;
//    RequestSDO(0x2001);
//    WaitForSdo(0x2001);

    SetPDO1On(FALSE); // this is a workaround for the Macs
    SetPDO1On(TRUE);

    SetNoWait(TRUE);
}

void Macs::StopMotor()
{
    //
    // Stop the motor and switch off the position control unit
    //
    SendSDO(0x3000, string('S','T','O','P'));
    WaitForSdo(0x3000);
}

void Macs::StopDevice()
{
    SetNoWait(FALSE);

    //
    // FIXME: This isn't called if the initialization isn't done completely!
    //

    SetRpmMode(FALSE);

    SetPDO1On(FALSE);

    lout << "- Motor off of " << (int)GetId() << endl;
    SendSDO(0x3000, string('O', 'F', 'F'));
    WaitForSdo(0x3000);

    /*
     lout << "- Stopping Program of " << (int)GetId() << endl;
     SendSDO(0x4000, (LWORD_t)0xaffe);
     WaitForSdo();
    */
}

void Macs::ReqPos()
{
    lout << "- Requesting Position of #" << (int)GetId() << endl;
    RequestSDO(0x6004);
    WaitForSdo(0x6004);
}

void Macs::ReqVel()
{
    lout << "- Requesting Velocity of #" << (int)GetId() << endl;
    RequestSDO(0x2002);
    WaitForSdo(0x2002);
}

void Macs::SetHome(LWORDS_t pos)
{
    lout << "- Driving #" << (int)GetId() << " to home position, Offset=" << dec << pos << endl;
    SendSDO(0x6003, 2, (LWORD_t)pos);       // home
    WaitForSdo(0x6003, 2);

    SendSDO(0x3001, string('h','o','m','e'));       // home
    WaitForSdo(0x3001);
    lout << "- Home position of #" << (int)GetId() << " reached. " << endl;
}

void Macs::SetVelocity(LWORD_t vel)
{
    SendSDO(0x2002, vel);     // velocity
    WaitForSdo(0x2002);
}

void Macs::SetAcceleration(LWORD_t acc)
{
    SendSDO(0x2003, 0, acc);     // acceleration
    WaitForSdo(0x2003, 0);
}

void Macs::SetDeceleration(LWORD_t dec)
{
    SendSDO(0x2003, 1, dec);     // acceleration
    WaitForSdo(0x2003, 1);
}

void Macs::SetRpmMode(BYTE_t mode)
{
    //
    // SetRpmMode(FALSE) stop the motor, but lets the position control unit on
    //

    SendSDO(0x3006, 0, mode ? string('S','T','R','T') : string('S','T','O','P'));
    WaitForSdo(0x3006, 0);
}

void Macs::SetRpmVelocity(LWORDS_t cvel)
{
    SendSDO(0x3006, 1, (LWORD_t)cvel);
    WaitForSdo(0x3006, 1);
}

void Macs::StartRelPos(LWORDS_t pos)
{
    SendSDO(0x6004, 1, (LWORD_t)pos);
}

void Macs::StartAbsPos(LWORDS_t pos)
{
    SendSDO(0x6004, 0, (LWORD_t)pos);
}

void Macs::SetNoWait(BYTE_t flag)
{
    lout << "- Setting NOWAIT " << (flag?"ON":"OFF") << " #" << (int)GetId() << endl;
    SendSDO(0x3008, flag ? string('O', 'N') : string('O', 'F', 'F'));
    WaitForSdo(0x3008);
}
/*
void Macs::ReqAxEnd()
{
    RequestSDO(0x2001);
    WaitForSdo(0x2001);
}
*/
void Macs::SendMsg(BYTE_t data[6])
{
    GetNetwork()->SendCanFrame(fMacId, 0, 0, data[0], data[1], data[2], data[3], data[4], data[5]);
}

void Macs::SendMsg(BYTE_t d0=0, BYTE_t d1=0, BYTE_t d2=0,
                   BYTE_t d3=0, BYTE_t d4=0, BYTE_t d5=0)
{
    GetNetwork()->SendCanFrame(fMacId, 0, 0, d0, d1, d2, d3, d4, d5);
}

void Macs::HandlePDO1(BYTE_t *data, struct timeval *tv)
{
    fPdoPos    = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];

    fPosActive = data[3]&0x02;
    fRpmActive = data[3]&0x04;

    fPdoTime.SetTimer(tv);
}

void Macs::HandlePDO2(BYTE_t *data, struct timeval *tv)
{
    LWORDS_t errnum = (data[0]<<24) | (data[1]<<16) | (data[2]<<8) | data[3];
    LWORDS_t errinf = (data[4]<<24) | (data[5]<<16) | (data[6]<<8) | data[7];

    if (!errnum)
    {
        cout << "Mac #" << (int)GetId() << " reports Error occursion." << endl;
        fError = TRUE;
        return;
    }

    cout << "Mac #" << (int)GetId() << " reports: ";
    switch (errnum)
    {
    case 6:
        cout << "Home position not the first positioning command." << endl;
        return;

    case 8:
        cout << "Control deviation overflow." << endl;
        return;

    case 9:
        cout << "Zero index not found." << endl;
        return;

    case 25:
        switch (errinf)
        {
        case 1:
            cout << "Positive";
            break;
        case 2:
            cout << "Negative";
            break;
        default:
            cout << "-unknown-";
        }
        cout << " endswitch activated." << endl;
        fError = FALSE;
        return;

    case 84:
        cout << "Too many (>12) ON TIME calls." << endl;
        return;

    default:
        cout << "Error Nr. " << errnum << ", " << errinf << endl;
    }
}

double Macs::GetTime()
{
    return fPosTime.GetTime();
}

double Macs::GetMjd()
{
    return fPosTime.GetMjd();
}

double Macs::GetPdoTime()
{
    return fPdoTime.GetTime();
}

double Macs::GetPdoMjd()
{
    return fPdoTime.GetMjd();
}