#include "ccsds.h"

ccsds::ccsds(unsigned char data_field_header,
             unsigned char processID,
             unsigned int sequence_count,
             unsigned int packet_length,
             unsigned char acceptance,
             unsigned int completion,
             unsigned char service_type,
             unsigned char service_subtype,
             unsigned char sourceID)
{
    unsigned int packetID = 0;
    ccsds_header = new ccsds_telecommand_source_packet_hdr_t ;

    packetID = packetID + (0b000<<13);              // version number
    packetID = packetID + (0b1<<12);                // packet type
    packetID = packetID + (data_field_header<<11);  // data field header    1 if there is a datafield header
    packetID = packetID + (processID<<4);           // process ID           7 bits
    packetID = packetID + (0b1100);                 // packet category      '1100'

    ccsds_header->packetId1 = (unsigned char) (packetID>>8);
    ccsds_header->packetId0 = (unsigned char) (packetID);

    ccsds_header->packetSequenceControl1 = (unsigned char) ((0b11<<6) + (sequence_count>>8));
    ccsds_header->packetSequenceControl0 = (unsigned char) (sequence_count);

    ccsds_header->packetLength1 = (unsigned char) (packet_length>>8);
    ccsds_header->packetLength0 = (unsigned char) (packet_length);

    ccsds_header->dataFieldHeader3 = ccsds_header->dataFieldHeader3 + (0b0001<<4);
    ccsds_header->dataFieldHeader3 = ccsds_header->dataFieldHeader3 + (completion<<3);
    ccsds_header->dataFieldHeader3 = ccsds_header->dataFieldHeader3 + (acceptance);
    ccsds_header->dataFieldHeader2 = service_type;
    ccsds_header->dataFieldHeader1 = service_subtype;
    ccsds_header->dataFieldHeader0 = sourceID;
}