#include "ObjectSignal.h"

#include <iostream>


Object_outlines_outline::Object_outlines_outline()
{

}

Object_outlines::Object_outlines()
{

}

Object_material::Object_material()
{

}

int Object_material::Getsurface(string & surface)
{
    if(msurface.size()<1)return 0;
    surface = msurface[0];
    return 1;
}

int Object_material::Getfriction(double & friction)
{
    if(mfriction.size()<1)return 0;
    friction = mfriction[0];
    return 1;
}

int Object_material::Getroughness(double & roughness)
{
    if(mroughness.size()<1)return 0;
    roughness = mroughness[0];
    return 1;
}

void Object_material::Setsurface(string surface)
{
    if(msurface.size()>0)msurface.clear();
    msurface.push_back(surface);
}

void Object_material::Setfriction(double friction)
{
    if(mfriction.size()>0)mfriction.clear();
    mfriction.push_back(friction);
}

void Object_material::Setroughness(double roughness)
{
    if(mroughness.size()>0)mroughness.clear();
    mroughness.push_back(roughness);
}

Object_repeat::Object_repeat(double s,double length,double distance,double tStart,double tEnd,double heightStart,
              double heightEnd,double zOffsetStart,double zOffsetEnd)
{
    ms = s;
    mlength = length;
    mdistance = distance;
    mtStart = tStart;
    mtEnd = tEnd;
    mheightStart = heightStart;
    mheightEnd = heightEnd;
    mzOffsetStart = zOffsetStart;
    mzOffsetEnd = zOffsetEnd;
}

double Object_repeat::Gets()
{
    return ms;
}

double Object_repeat::Getlength()
{
    return mlength;
}

double Object_repeat::Getdistance()
{
    return mdistance;
}

double Object_repeat::GettStart()
{
    return mtStart;
}

double Object_repeat::GettEnd()
{
    return mtEnd;
}

double Object_repeat::GetheightStart()
{
    return mheightStart;
}

double Object_repeat::GetheightEnd()
{
    return mheightEnd;
}

double Object_repeat::GetzOffsetStart()
{
    return mzOffsetStart;
}

double Object_repeat::GetzOffsetEnd()
{
    return mzOffsetEnd;
}


void Object_repeat::Sets(double s)
{
    ms = s;
}

void Object_repeat::Setlength(double length)
{
    mlength = length;
}

void Object_repeat::Setdistance(double distance)
{
    mdistance = distance;
}

void Object_repeat::SettStart(double tStart)
{
    mtStart = tStart;
}

void Object_repeat::SettEnd(double tEnd)
{
    mtEnd = tEnd;
}

void Object_repeat::SetheightStart(double heightStart)
{
    mheightStart = heightStart;
}

void Object_repeat::SetheightEnd(double heightEnd)
{
    mheightEnd = heightEnd;
}

void Object_repeat::SetzOffsetStart(double zOffsetStart)
{
    mzOffsetStart = zOffsetStart;
}

void Object_repeat::SetzOffsetEnd(double zOffsetEnd)
{
    mzOffsetEnd = zOffsetEnd;
}


int Object_repeat::GetwidthStart(double & widthStart)
{
    if(mwidthStart.size() <1)return 0;
    widthStart = mwidthStart[0];
    return 1;
}

int Object_repeat::GetwidthEnd(double & widthEnd)
{
    if(mwidthEnd.size() < 1)return 0;
    widthEnd = mwidthEnd[0];
    return 1;
}

int Object_repeat::GetlengthStart(double & lengthStart)
{
    if(mlengthStart.size() < 1)return 0;
    lengthStart = mlengthStart[0];
    return 1;
}

int Object_repeat::GetlengthEnd(double & lengthEnd)
{
    if(mlengthEnd.size() < 1)return 0;
    lengthEnd = mlengthEnd[0];
    return 1;
}

int Object_repeat::GetradiusStart(double & radiusStart)
{
    if(mradiusStart.size() < 1)return 0;
    radiusStart = mradiusStart[0];
    return 1;
}

int Object_repeat::GetradiusEnd(double & radiusEnd)
{
    if(mradiusEnd.size() < 1)return 0;
    radiusEnd = mradiusEnd[0];
    return 1;
}

void Object_repeat::DeletewidthStart()
{
    if(mwidthStart.size() > 0)mwidthStart.clear();
}

void Object_repeat::DeletewidthEnd()
{
    if(mwidthEnd.size() > 0)mwidthEnd.clear();
}

void Object_repeat::DeletelengthStart()
{
    if(mlengthStart.size() > 0)mlengthStart.clear();
}

void Object_repeat::DeletelengthEnd()
{
    if(mlengthEnd.size() > 0)mlengthEnd.clear();
}

void Object_repeat::DeleteradiusStart()
{
    if(mradiusStart.size() > 0)mradiusStart.clear();
}

void Object_repeat::DeleteradiusEnd()
{
    if(mradiusEnd.size() > 0)mradiusEnd.clear();
}

void Object_repeat::SetwidthStart(double  widthStart)
{
    if(mwidthStart.size() > 0)mwidthStart.clear();
    mwidthStart.push_back(widthStart);
}

void Object_repeat::SetwidthEnd(double  widthEnd)
{
    if(mwidthEnd.size() > 0)mwidthEnd.clear();
    mwidthEnd.push_back(widthEnd);
}

void Object_repeat::SetlengthStart(double  lengthStart)
{
    if(mlengthStart.size() > 0)mlengthStart.clear();
    mlengthStart.push_back(lengthStart);
}

void Object_repeat::SetlengthEnd(double  lengthEnd)
{
    if(mlengthEnd.size() > 0)mlengthEnd.clear();
    mlengthEnd.push_back(lengthEnd);
}

void Object_repeat::SetradiusStart(double  radiusStart)
{
    if(mradiusStart.size() > 0)mradiusStart.clear();
    mradiusStart.push_back(radiusStart);
}

void Object_repeat::SetradiusEnd(double  radiusEnd)
{
    if(mradiusEnd.size() > 0)mradiusEnd.clear();
    mradiusEnd.push_back(radiusEnd);
}

/**
* Check if the tested s-offset is inside the lane offset interval
* @param A double s-offset value that has to be checked
* @return Return true if the s-offset value belongs to current lane section, false otherwise
*/
bool Object_repeat::CheckInterval(double s_check)
{
    if (s_check>=ms)
        return true;
    else
        return false;
}

Object_parkingSpace::Object_parkingSpace()
{
    maccess = "all";
}

void Object_parkingSpace::Setaccess(std::string access)
{
    maccess = access;
}

void Object_parkingSpace::Setrestrictions(std::string restrictions)
{
    mrestrictions = restrictions;
}

string Object_parkingSpace::Getaccess()
{
    return maccess;
}

string Object_parkingSpace::Getrestrictions()
{
    return mrestrictions;
}

/**
 * Copy constructor
 */
Object_parkingSpace::Object_parkingSpace (const Object_parkingSpace& parkingSpace)
{
    if(this != &parkingSpace)
    {
        mrestrictions = parkingSpace.mrestrictions;
        maccess = parkingSpace.maccess;
    }
}

/**
 * Assignment operator overload
 */
const Object_parkingSpace& Object_parkingSpace::operator=(const Object_parkingSpace& rhs)
{
    if (this!= &rhs)
    {
        mrestrictions = rhs.mrestrictions;
        maccess = rhs.maccess;
    }
}

Object::Object(std::string id, double s, double t, double zOffset)
{
    mid = id;
    ms = s;
    mt = t;
    mzOffset = zOffset;
}

double Object::Gett()
{
    return mt;
}

double Object::GetzOffset()
{
    return mzOffset;
}

string Object::Gettype()
{
    return mtype;
}

int Object::GetvalidLength(double &validLength)
{
    if(mvalidLength.size()<1)return 0;
    validLength = mvalidLength[0];
    return 1;
}

string Object::Getorientation()
{
    return morientation;
}

string Object::Getsubtype()
{
    return msubtype;
}

string Object::Getdynamic()
{
    return mdynamic;
}

int Object::Gethdg(double &hdg)
{
    if(mhdg.size()<1)return 0;
    hdg = mhdg[0];
    return 1;
}

string Object::Getname()
{
    return mname;
}

int Object::Getpitch(double &pitch)
{
    if(mpitch.size()<1)return 0;
    pitch = mpitch[0];
    return 1;
}

string Object::Getid()
{
    return mid;
}

int Object::Getroll(double &roll)
{
    if(mroll.size()<1)return 0;
    roll = mroll[0];
    return 1;
}

int Object::Getheight(double &height)
{
    if(mheight.size()<1)return 0;
    height = mheight[0];
    return 1;
}

double Object::Gets()
{
    return ms;
}

int Object::Getlength(double &length)
{
    if(mlength.size()<1)return 0;
    length = mlength[0];
    return 1;
}

int Object::Getwidth(double &width)
{
    if(mwidth.size()<1)return 0;
    width = mwidth[0];
    return 1;
}

int Object::Getradius(double &radius)
{
    if(mradius.size()<1)return 0;
    radius = mradius[0];
    return 1;
}

int Object::GetparkingSpace(Object_parkingSpace &parkingSpace)
{
    if(mObject_parkingSpace.size()<1)return 0;
    parkingSpace = mObject_parkingSpace[0];
    return 1;
}

void Object::Sett(double t)
{
    mt = t;
}

void Object::SetzOffset(double zOffset)
{
    mzOffset = zOffset;
}
void Object::Settype(string type)
{
    mtype = type;
}

void Object::SetvalidLength(double  validLength)
{
    if(mvalidLength.size() > 0)mvalidLength.clear();
    mvalidLength.push_back(validLength);
}

void Object::Setorientation(string orientation)
{
    morientation = orientation;
}

void Object::Setsubtype(string subtype)
{
    msubtype = subtype;
}

void Object::Setdynamic(string dynamic)
{
    mdynamic = dynamic;
}

void Object::Sethdg(double  hdg)
{
    if(mhdg.size()>0)mhdg.clear();
    mhdg.push_back(hdg);
}

void Object::Setname(string name)
{
    mname = name;
}

void Object::Setpitch(double pitch)
{
    if(mpitch.size()>0)mpitch.clear();
    mpitch.push_back(pitch);
}

void Object::Setid(string id)
{
    mid = id;
}

void Object::Setroll(double  roll)
{
    if(mroll.size()>0)mroll.clear();
    mroll.push_back(roll);
}

void Object::Setheight(double height)
{
    if(mheight.size()>0)mheight.clear();
    mheight.push_back(height);
}

void Object::Sets(double s)
{
    ms = s;
}

void Object::Setlength(double length)
{
    if(mlength.size()>0)mlength.clear();
    mlength.push_back(length);
}

void Object::Setwidth(double width)
{
    if(mwidth.size()>0)mwidth.clear();
    mwidth.push_back(width);
}

void Object::Setradius(double radius)
{
    if(mradius.size()>0)mradius.clear();
    mradius.push_back(radius);
}

void Object::SetparkingSpace(Object_parkingSpace parkingSpace)
{
    if(mObject_parkingSpace.size()>0)mObject_parkingSpace.clear();
    mObject_parkingSpace.push_back(parkingSpace);
}

vector<Object_repeat> * Object::GetObjectrepeatVector()
{
    return &mObject_repeat;
}

Object_repeat* Object::GetObjectrepeat(unsigned int i)
{
    if ((mObject_repeat.size()>0)&&(i<(mObject_repeat.size())))
        return &(mObject_repeat.at(i));
    else
        return NULL;
}

unsigned int Object::GetObjectrepeatCount()
{
    return mObject_repeat.size();
}

Object_repeat*			Object::GetLastObjectrepeat()
{
    if (mObject_repeat.size()>0)
        return &mObject_repeat.at(mObject_repeat.size()-1);
    else
        return NULL;
}

Object_repeat*			Object::GetLastAddedObjectrepeat()
{
    if(mLastAddedObjectrepeat<mObject_repeat.size())
        return &mObject_repeat.at(mLastAddedObjectrepeat);
    else
        return NULL;
}

unsigned int Object::AddObjectrepeat(double s,double length,double distance,double tStart,double tEnd,
                             double heightStart,double heightEnd,double zOffsetStart,double zOffsetEnd)
{
    unsigned int index = CheckObjectrepeatInterval(s)+1;
    if(index>=GetObjectrepeatCount()) mObject_repeat.push_back(Object_repeat(s,length,distance,tStart,tEnd,heightStart,heightEnd,zOffsetStart,zOffsetEnd));
    else mObject_repeat.insert(mObject_repeat.begin()+index, Object_repeat(s,length,distance,tStart,tEnd,heightStart,heightEnd,zOffsetStart,zOffsetEnd));
    mLastAddedObjectrepeat=index;
    return index;
}

unsigned int Object::CloneObjectrepeat(unsigned int index)
{
    if(index<(mObject_repeat.size()-1))
        mObject_repeat.insert(mObject_repeat.begin()+index+1, mObject_repeat[index]);
    else if(index==mObject_repeat.size()-1)
        mObject_repeat.push_back(mObject_repeat[index]);
    mLastAddedObjectrepeat=index+1;
    return mLastAddedObjectrepeat;
}

void Object::DeleteObjectrepeat(unsigned int index)
{
    mObject_repeat.erase(mObject_repeat.begin()+index);
}

int Object::CheckObjectrepeatInterval(double s_check)
{
    int res=-1;
    //Go through all the lane section records
    for (unsigned int i=0;i<mObject_repeat.size();i++)
    {
        //check if the s_check belongs to the current record
        if (mObject_repeat.at(i).CheckInterval(s_check))
            res=i;	//assign it to the result id
        else
            break;	//if not, break;
    }
    return res;		//return the result: 0 to MaxInt as the index to the record containing s_check or -1 if nothing found

}

vector<Object_material> * Object::GetObjectmaterialVector()
{
    return &mObject_material;
}

Object_material* Object::GetObjectmaterial(unsigned int i)
{
    if ((mObject_material.size()>0)&&(i<(mObject_material.size())))
        return &(mObject_material.at(i));
    else
        return NULL;
}
unsigned int Object::GetObjectmaterialCount()
{
    return mObject_material.size();
}

Object_material*			Object::GetLastObjectmaterial()
{
    if (mObject_material.size()>0)
        return &mObject_material.at(mObject_material.size()-1);
    else
        return NULL;
}

Object_material*			Object::GetLastAddedObjectmaterial()
{
    if(mLastAddedObjectmaterial<mObject_material.size())
        return &mObject_material.at(mLastAddedObjectmaterial);
    else
        return NULL;
}

unsigned int Object::AddObjectmaterial()
{
    mObject_material.push_back(Object_material());
    mLastAddedObjectmaterial = mObject_material.size()-1;
    return mLastAddedObjectmaterial;
}

unsigned int Object::CloneObjectmaterial(unsigned int index)
{
    if(index<(mObject_material.size()-1))
        mObject_material.insert(mObject_material.begin()+index+1, mObject_material[index]);
    else if(index==mObject_material.size()-1)
        mObject_material.push_back(mObject_material[index]);
    mLastAddedObjectmaterial=index+1;
    return mLastAddedObjectmaterial;
}

void Object::DeleteObjectmaterial(unsigned int index)
{
    mObject_material.erase(mObject_material.begin()+index);
}


signal_positionRoad::signal_positionRoad(double s, double t, double zOffset, double hOffset, double pitch, double roll)
{
    ms = s;
    mt = t;
    mzOffset = zOffset;
    mhOffset = hOffset;
    mpitch = pitch;
    mroll = roll;
}

double signal_positionRoad::Gets()
{
    return ms;
}

double signal_positionRoad::Gett()
{
    return mt;
}

double signal_positionRoad::GetzOffset()
{
    return mzOffset;
}

double signal_positionRoad::GethOffset()
{
    return mhOffset;
}

double signal_positionRoad::Getpitch()
{
    return mpitch;
}

double signal_positionRoad::Getroll()
{
    return mroll;
}

void signal_positionRoad::Sets(double s)
{
    ms = s;
}

void signal_positionRoad::Sett(double t)
{
    mt = t;
}

void signal_positionRoad::SetzOffset(double zOffset)
{
    mzOffset = zOffset;
}

void signal_positionRoad::SethOffset(double hOffset)
{
    mhOffset = hOffset;
}

void signal_positionRoad::Setpitch(double pitch)
{
    mpitch = pitch;
}

void signal_positionRoad::Setroll(double roll)
{
    mroll = roll;
}

signal_positionInertial::signal_positionInertial(double x, double y,double z, double hdg, double pitch, double roll)
{
    mx = x;
    my = y;
    mz = z;
    mhdg = hdg;
    mpitch = pitch;
    mroll = roll;
}

double signal_positionInertial::Getx()
{
    return mx;
}

double signal_positionInertial::Gety()
{
    return my;
}

double signal_positionInertial::Getz()
{
    return mz;
}

double signal_positionInertial::Gethdg()
{
    return mhdg;
}

double signal_positionInertial::Getpitch()
{
    return mpitch;
}

double signal_positionInertial::Getroll()
{
    return mroll;
}

void signal_positionInertial::Setx(double x)
{
    mx = x;
}

void signal_positionInertial::Sety(double y)
{
    my = y;
}

void signal_positionInertial::Setz(double z)
{
    mz = z;
}

void signal_positionInertial::Sethdg(double hdg)
{
    mhdg = hdg;
}

void signal_positionInertial::Setpitch(double pitch)
{
    mpitch = pitch;
}

void signal_positionInertial::Setroll(double roll)
{
    mroll = roll;
}


signal_laneValidity::signal_laneValidity(int fromLane,int toLane)
{
    mfromLane = fromLane;
    mtoLane = toLane;
}

int signal_laneValidity::GetfromLane()
{
    return mfromLane;
}

int signal_laneValidity::GettoLane()
{
    return mtoLane;
}

void signal_laneValidity::SetfromLane(int fromLane)
{
    mfromLane = fromLane;
}

void signal_laneValidity::SettoLane(int toLane)
{
    mtoLane = toLane;
}


Signal::Signal(double s, double t, std::string id, std::string name, bool dynamic,string orientation,
               double zOffset, string type, std::string country, std::string countryRevision,
               string subtype, double hOffset, double pitch, double roll, double height, double width)
{
    ms = s;
    mt = t;
    mid = id;
    mname = name;
    mdynamic = dynamic;
    morientation = orientation;
    mzOffset = zOffset;
    mtype = type;
    mcountry = country;
    mcountryRevision = countryRevision;
    msubtype = subtype;
    mhOffset = hOffset;
    mpitch = pitch;
    mroll = roll;
    mheight = height;
    mwidth = width;
    mpsignal_laneValidity = 0;
    mpsignal_positionInertial = 0;
    mpsignal_positionRoad = new signal_positionRoad(s,t,zOffset,hOffset,pitch,roll);
}

Signal::Signal()
{
    mpsignal_positionInertial = 0;
    mpsignal_positionRoad = 0;
    mpsignal_laneValidity = 0;

}
Signal::~Signal()
{
    if(mpsignal_laneValidity != 0)delete mpsignal_laneValidity;
    if(mpsignal_positionInertial != 0)delete mpsignal_positionInertial;
    if(mpsignal_positionRoad != 0)delete mpsignal_positionRoad;
}

Signal& Signal::operator=(const Signal& x)
{
    if (this != &x)
    {
        this->ms = x.ms;
        this->mt = x.mt;
        this->mid = x.mid;
        this->mname = x.mname;
        this->mdynamic = x.mdynamic;
        this->morientation = x.morientation;
        this->mzOffset = x.mzOffset;
        this->mtype = x.mtype;
        this->mcountry = x.mcountry;
        this->mcountryRevision = x.mcountryRevision;
        this->msubtype = x.msubtype;
        this->mhOffset = x.mhOffset;
        this->mpitch = x.mpitch;
        this->mroll = x.mroll;
        this->mheight = x.mheight;
        this->mwidth = x.mwidth;
        this->mpsignal_positionInertial = 0;
        if(x.mpsignal_positionInertial != 0)
        {
            this->mpsignal_positionInertial = new signal_positionInertial(x.mpsignal_positionInertial->Getx(),
                                                                      x.mpsignal_positionInertial->Gety(),
                                                                      x.mpsignal_positionInertial->Getz(),
                                                                      x.mpsignal_positionInertial->Gethdg(),
                                                                      x.mpsignal_positionInertial->Getpitch(),
                                                                      x.mpsignal_positionInertial->Getroll());
        }
        this->mpsignal_laneValidity = 0;
        if(x.mpsignal_laneValidity != 0)
        {
            this->mpsignal_laneValidity = new signal_laneValidity(x.mpsignal_laneValidity->GetfromLane(),
                                                              x.mpsignal_laneValidity->GettoLane());
        }
        this->mpsignal_positionRoad = new signal_positionRoad(ms,mt,mzOffset,mhOffset,mpitch,mroll);
    }
    return *this;
}

Signal::Signal(const Signal &x)
{
    ms = x.ms;
    mt = x.mt;
    mid = x.mid;
    mname = x.mname;
    mdynamic = x.mdynamic;
    morientation = x.morientation;
    mzOffset = x.mzOffset;
    mtype = x.mtype;
    mcountry = x.mcountry;
    mcountryRevision = x.mcountryRevision;
    msubtype = x.msubtype;
    mhOffset = x.mhOffset;
    mpitch = x.mpitch;
    mroll = x.mroll;
    mheight = x.mheight;
    mwidth = x.mwidth;
    this->mpsignal_positionInertial = 0;
    if(x.mpsignal_positionInertial != 0)
    {
        this->mpsignal_positionInertial = new signal_positionInertial(x.mpsignal_positionInertial->Getx(),
                                                                  x.mpsignal_positionInertial->Gety(),
                                                                  x.mpsignal_positionInertial->Getz(),
                                                                  x.mpsignal_positionInertial->Gethdg(),
                                                                  x.mpsignal_positionInertial->Getpitch(),
                                                                  x.mpsignal_positionInertial->Getroll());
    }
    this->mpsignal_laneValidity = 0;
    if(x.mpsignal_laneValidity != 0)
    {
        this->mpsignal_laneValidity = new signal_laneValidity(x.mpsignal_laneValidity->GetfromLane(),
                                                          x.mpsignal_laneValidity->GettoLane());
    }
    mpsignal_positionRoad = new signal_positionRoad(ms,mt,mzOffset,mhOffset,mpitch,mroll);
}

double Signal::Gets()
{
    return ms;
}

double Signal::Gett()
{
    return mt;
}

string Signal::Getid()
{
    return mid;
}

string Signal::Getname()
{
    return mname;
}

bool Signal::Getdynamic()
{
    return mdynamic;
}

string Signal::Getorientation()
{
    return morientation;
}

double Signal::GetzOffset()
{
    return mzOffset;
}

string Signal::Gettype()
{
    return mtype;
}

string Signal::Getcountry()
{
    return mcountry;
}

string Signal::GetcountryRevision()
{
    return mcountryRevision;
}

string Signal::Getsubtype()
{
    return msubtype;
}

double Signal::GethOffset()
{
    return mhOffset;
}

double Signal::Getpitch()
{
    return mpitch;
}

double Signal::Getroll()
{
    return mroll;
}

double Signal::Getheight()
{
    return mheight;
}

double Signal::Getwidth()
{
    return mwidth;
}

signal_positionRoad * Signal::GetpositionRoad()
{
    return mpsignal_positionRoad;
}

signal_positionInertial * Signal::GetpositionInertial()
{
    return mpsignal_positionInertial;
}

void Signal::Sets(double s)
{
    ms = s;
}

void Signal::Sett(double t)
{
    mt = t;
}

void Signal::Setid(std::string id)
{
    mid = id;
}

void Signal::Setname(std::string name)
{
    mname = name;
}

void Signal::Setdynamic(bool dynamic)
{
    mdynamic = dynamic;
}

void Signal::Setorientation(std::string orientation)
{
    morientation = orientation;
}

void Signal::SetzOffset(double zOffset)
{
    mzOffset = zOffset;
}

void Signal::Settype(string type)
{
    mtype = type;
}

void Signal::Setcountry(std::string country)
{
    mcountry = country;
}

void Signal::SetcountryRevision(std::string countryRevision)
{
    mcountryRevision = countryRevision;
}

void Signal::Setsubtype(string subtype)
{
    msubtype = subtype;
}

void Signal::SethOffset(double hOffset)
{
    mhOffset = hOffset;
}

void Signal::Setpitch(double pitch)
{
    mpitch = pitch;
}

void Signal::Setroll(double roll)
{
    mroll = roll;
}

void Signal::Setheight(double height)
{
    mheight = height;
}

void Signal::Setwidth(double width)
{
    mwidth = width;
}

void Signal::SetlaneValidity(int fromLane, int toLane)
{
    if(mpsignal_laneValidity == 0)
    {
        mpsignal_laneValidity = new signal_laneValidity(fromLane,toLane);
    }
    else
    {
        mpsignal_laneValidity->SetfromLane(fromLane);
        mpsignal_laneValidity->SettoLane(toLane);
    }
}

void Signal::SetpositionRoad(double s, double t, double zOffset, double hOffset, double pitch,double roll)
{
    if(mpsignal_positionRoad == 0)
    {
        mpsignal_positionRoad = new signal_positionRoad(s,t,zOffset,hOffset,pitch,roll);
    }
    else
    {
        mpsignal_positionRoad->Sets(s);
        mpsignal_positionRoad->Sett(t);
        mpsignal_positionRoad->SetzOffset(zOffset);
        mpsignal_positionRoad->SethOffset(hOffset);
        mpsignal_positionRoad->Setpitch(pitch);
        mpsignal_positionRoad->Setroll(roll);
    }
}

void Signal::SetpositionInertial(double x, double y, double z, double hdg, double pitch, double roll)
{
    if(mpsignal_positionInertial == 0)
    {
        mpsignal_positionInertial = new signal_positionInertial(x,y,z,hdg,pitch,roll);
    }
    else
    {
        mpsignal_positionInertial->Setx(x);
        mpsignal_positionInertial->Sety(y);
        mpsignal_positionInertial->Setz(z);
        mpsignal_positionInertial->Sethdg(hdg);
        mpsignal_positionInertial->Setpitch(pitch);
        mpsignal_positionInertial->Setroll(roll);
    }
}

signal_laneValidity * Signal::GetlaneValidity()
{
    return mpsignal_laneValidity;
}