#include "autoroadcontact.h"

#include "mainwindow.h"

#include <math.h>
#include <iostream>

extern MainWindow * gw;

#include "xodrfunc.h"
#include "xodrdijkstra.h"

AutoRoadContact::AutoRoadContact()
{

}

/**
 * @brief AutoRoadContact::CalcContact
 * @param pRoad1
 * @param pRoad2
 * @param contacttype 0:start to start  1:start to end 2:end to start 3:end to end
 * @param turnstraight  0: turn  1:straight 2:u-turn
 * @param xARCLane   right lane contact
 * @param xARCOpLane left lane contact
 * @param fDisTolerance    distance must below this value
 * @return
 */
int AutoRoadContact::CalcContact(Road *pRoad1, Road *pRoad2,int & contacttype,int & turnstraight,
                                 std::vector<iv::ARC> & xARCLane,std::vector<iv::ARC> & xARCOpLane, const double fDisTolerance)
{

    double road1_start_x,road1_start_y,road1_end_x,road1_end_y,road1_start_hdg,road1_end_hdg;
    double road2_start_x,road2_start_y,road2_end_x,road2_end_y,road2_start_hdg,road2_end_hdg;
    int ngeo = pRoad1->GetGeometryCoords(0,road1_start_x,road1_start_y,road1_start_hdg);
    ngeo+=pRoad1->GetGeometryCoords(pRoad1->GetRoadLength(),road1_end_x,road1_end_y,road1_end_hdg);
    ngeo+=pRoad2->GetGeometryCoords(0,road2_start_x,road2_start_y,road2_start_hdg);
    ngeo+=pRoad2->GetGeometryCoords(pRoad2->GetRoadLength(),road2_end_x,road2_end_y,road2_end_hdg);
    if(ngeo<0)
    {
        return -1;
    }

    double dis_1s_2s = sqrt(pow(road1_start_x-road2_start_x,2)
                            +pow(road1_start_y-road2_start_y,2));
    double dis_1s_2e = sqrt(pow(road1_start_x-road2_end_x,2)
                            +pow(road1_start_y-road2_end_y,2));
    double dis_1e_2s = sqrt(pow(road1_end_x-road2_start_x,2)
                            +pow(road1_end_y-road2_start_y,2));
    double dis_1e_2e = sqrt(pow(road1_end_x-road2_end_x,2)
                            +pow(road1_end_y-road2_end_y,2));
    double fdis[4];
    fdis[0] = dis_1s_2s;
    fdis[1] = dis_1s_2e;
    fdis[2] = dis_1e_2s;
    fdis[3] = dis_1e_2e;

    double fdismin = 100000;
    int index = -1;
    unsigned int i;
    for(i=0;i<4;i++)
    {
        if(fdismin>fdis[i])
        {
            fdismin = fdis[i];
            index = i;
        }
    }

    if(fdismin > fDisTolerance)
    {
        std::cout<<"dis is grater than Distance Tolerance. Tolerance is "<<fDisTolerance<<std::endl;
        return -3;
    }

    if(fdismin < 0.1)
    {
        std::cout<<"dis is less than 0.1"<<std::endl;
        return -4;
    }

    contacttype = index;

    if(contacttype == -1)
    {
        return -2;
    }

    LaneSection * pLS1 = 0;
    LaneSection * pLS2 = 0;
    double from_x,from_y,from_hdg,to_x,to_y,to_hdg;
    switch (contacttype) {
    case 0:
        from_x = road1_start_x;
        from_y = road1_start_y;
        to_x = road2_start_x;
        to_y = road2_start_y;
        from_hdg = road1_start_hdg + M_PI;
        to_hdg = road2_start_hdg;
        pLS1 = pRoad1->GetLaneSection(0);
        pLS2 = pRoad2->GetLaneSection(0);
        break;
    case 1:
        from_x = road1_start_x;
        from_y = road1_start_y;
        to_x = road2_end_x;
        to_y = road2_end_y;
        from_hdg = road1_start_hdg + M_PI;
        to_hdg = road2_end_hdg+ M_PI;
        pLS1 = pRoad1->GetLaneSection(0);
        pLS2 = pRoad2->GetLastLaneSection();
        break;
    case 2:
        from_x = road1_end_x;
        from_y = road1_end_y;
        to_x = road2_start_x;
        to_y = road2_start_y;
        from_hdg = road1_end_hdg ;
        to_hdg = road2_start_hdg;
        pLS1 = pRoad1->GetLastLaneSection();
        pLS2 = pRoad2->GetLaneSection(0);
        break;
    case 3:
        from_x = road1_end_x;
        from_y = road1_end_y;
        to_x = road2_end_x;
        to_y = road2_end_y;
        from_hdg = road1_end_hdg ;
        to_hdg = road2_end_hdg + M_PI;
        pLS1 = pRoad1->GetLastLaneSection();
        pLS2 = pRoad2->GetLastLaneSection();
        break;
    default:
        break;
    }

    if(from_hdg >= 2.0*M_PI)from_hdg = from_hdg - 2.0*M_PI;
    if(to_hdg >= 2.0*M_PI)to_hdg = to_hdg - 2.0*M_PI;

    double hdgdiff = to_hdg - from_hdg;
    while(hdgdiff>=2.0*M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
    while(hdgdiff<0)hdgdiff = hdgdiff + 2.0*M_PI;
    bool bTurnRight = true;
    if((hdgdiff<0.5)||(hdgdiff>(2.0*M_PI-0.5)))
    {
        turnstraight = 1;
    }
    else
    {
        if((hdgdiff>(M_PI-0.3))&&(hdgdiff<(M_PI+0.3)))
        {
            turnstraight = 2;
        }
        else
        {
            if(hdgdiff>M_PI)
            {
                std::cout<<"turn right"<<std::endl;
                bTurnRight = true;
            }
            else
            {
                std::cout<<"turn left"<<std::endl;
                bTurnRight = false;
            }
            turnstraight = 0;
        }
    }


    vector<Lane *> xroad1lane1,xroad1lane2;
    vector<Lane *> xroad2lane1,xroad2lane2;
    for(i=0;i<pLS1->GetLeftLaneCount();i++)
    {
        Lane * pLane = pLS1->GetLeftLaneAt(i+1);
        if(pLane == NULL)
        {
            std::cout<<"GetLeftLaneError i:"<<i<<std::endl;
            break;
        }
        xroad1lane1.push_back(pLane);
    }
    for(i=0;i<pLS1->GetRightLaneCount();i++)
    {
        Lane * pLane = pLS1->GetRightLaneAt(i+1);
        if(pLane == NULL)
        {
            std::cout<<"GetRightLaneAt i:"<<i<<std::endl;
            break;
        }
        xroad1lane2.push_back(pLane);
    }
    for(i=0;i<pLS2->GetLeftLaneCount();i++)
    {
        Lane * pLane = pLS2->GetLeftLaneAt(i+1);
        if(pLane == NULL)
        {
            std::cout<<"GetLeftLaneError i:"<<i<<std::endl;
            break;
        }
        xroad2lane1.push_back(pLane);
    }
    for(i=0;i<pLS2->GetRightLaneCount();i++)
    {
        Lane * pLane = pLS2->GetRightLaneAt(i+1);
        if(pLane == NULL)
        {
            std::cout<<"GetRightLaneAt i:"<<i<<std::endl;
            break;
        }
        xroad2lane2.push_back(pLane);
    }

    vector<Lane *> xroad1leftlane,xroad1rightlane;
    vector<Lane *> xroad2leftlane,xroad2rightlane;

    if((contacttype == 0)||(contacttype == 1))
    {
        xroad1leftlane = xroad1lane2;
        xroad1rightlane = xroad1lane1;
    }
    else
    {
        xroad1leftlane = xroad1lane1;
        xroad1rightlane = xroad1lane2;
    }

    if((contacttype == 1)||(contacttype == 3))
    {
        xroad2leftlane = xroad2lane2;
        xroad2rightlane = xroad2lane1;
    }
    else
    {
        xroad2leftlane = xroad2lane1;
        xroad2rightlane = xroad2lane2;
    }

    if(turnstraight == 1)  //
    {
        for(i=0;i<xroad1rightlane.size();i++)
        {
            Lane * pLane1 = xroad1rightlane.at(i);
            if(i>=xroad2rightlane.size())
            {
                break;
            }
            Lane * pLane2 = xroad2rightlane.at(i);
            if((pLane1 == NULL)||(pLane2 == NULL))
            {
                break;
            }
            if(pLane1->GetType() != pLane2->GetType())
            {
                break;
            }
            iv::ARC xARC;
            xARC.from = pLane1->GetId();
            xARC.to = pLane2->GetId();
            xARCLane.push_back(xARC);
        }
        for(i=0;i<xroad1leftlane.size();i++)
        {
            Lane * pLane1 = xroad1leftlane.at(i);
            if(i>=xroad2leftlane.size())
            {
                break;
            }
            Lane * pLane2 = xroad2leftlane.at(i);
            if((pLane1 == NULL)||(pLane2 == NULL))
            {
                break;
            }
            if(pLane1->GetType() != pLane2->GetType())
            {
                break;
            }
            iv::ARC xARC;
            xARC.from = pLane1->GetId();
            xARC.to = pLane2->GetId();
            xARCOpLane.push_back(xARC);
        }
    }
    else
    {
        if(turnstraight == 0)
        {
            int nindex1 = -1;
            int nindex2 = -1;
            for(i=0;i<xroad1rightlane.size();i++)
            {
                if(xroad1rightlane.at(i)->GetType() == "driving")
                {
                    nindex1 = i;
                    if(bTurnRight == false)
                    {
                        break;
                    }
                }
            }
            for(i=0;i<xroad2rightlane.size();i++)
            {
                if(xroad2rightlane.at(i)->GetType() == "driving")
                {
                    nindex2 = i;
                    if(bTurnRight == false)
                    {
                        break;
                    }
                }
            }
            if((nindex1<0)||(nindex2<0))
            {
                std::cout<<"can't find driving lane"<<std::endl;
                return -3;
            }
            if(bTurnRight)
            {
                for(i=nindex1;i<xroad1rightlane.size();i++)
                {
                    Lane * pLane1 = xroad1rightlane.at(i);
                    if(nindex2 >= xroad2rightlane.size())
                    {
                        break;
                    }
                    Lane * pLane2 = xroad2rightlane.at(nindex2);nindex2++;
                    if((pLane1 == NULL)||(pLane2 == NULL))
                    {
                        break;
                    }
                    if(pLane1->GetType() != pLane2->GetType())
                    {
                        break;
                    }
                    iv::ARC xARC;
                    xARC.from = pLane1->GetId();
                    xARC.to = pLane2->GetId();
                    xARCLane.push_back(xARC);
                }

            }
            else
            {
                Lane * pLane1 = xroad1rightlane.at(nindex1);
                Lane * pLane2 = xroad2rightlane.at(nindex2);
                if((pLane1 == NULL)||(pLane2 == NULL))
                {
                    std::cout<<"Lane NULL"<<std::endl;
                }
                if(pLane1->GetType() != pLane2->GetType())
                {
                    std::cout<<"Type Not Rigth"<<std::endl;
                }
                iv::ARC xARC;
                xARC.from = pLane1->GetId();
                xARC.to = pLane2->GetId();
                xARCLane.push_back(xARC);
            }
        }
        else
        {
            int nindex1 = -1;
            int nindex2 = -1;
            for(i=0;i<xroad1rightlane.size();i++)
            {
                if(xroad1rightlane.at(i)->GetType() == "driving")
                {
                    nindex1 = i;
                    break;
                }
            }
            for(i=0;i<xroad2rightlane.size();i++)
            {
                if(xroad2rightlane.at(i)->GetType() == "driving")
                {
                    nindex2 = i;
                    break;
                }
            }
            if((nindex1<0)||(nindex2<0))
            {
                std::cout<<"can't find driving lane"<<std::endl;
                return -3;
            }
            Lane * pLane1 = xroad1rightlane.at(nindex1);
            Lane * pLane2 = xroad2rightlane.at(nindex2);
            if((pLane1 == NULL)||(pLane2 == NULL))
            {
                std::cout<<"Lane NULL"<<std::endl;
            }
            if(pLane1->GetType() != pLane2->GetType())
            {
                std::cout<<"Type Not Rigth"<<std::endl;
            }
            iv::ARC xARC;
            xARC.from = pLane1->GetId();
            xARC.to = pLane2->GetId();
            xARCLane.push_back(xARC);
        }
    }

    std::cout<<"Lane Contact"<<std::endl;
    for(i=0;i<xARCLane.size();i++)
    {
        std::cout<<" From "<<xARCLane.at(i).from<<" To"<<xARCLane.at(i).to<<std::endl;
    }
    std::cout<<"OpLane Contact"<<std::endl;
    for(i=0;i<xARCOpLane.size();i++)
    {
        std::cout<<" From "<<xARCOpLane.at(i).from<<" To"<<xARCOpLane.at(i).to<<std::endl;
    }

    return 0;

}

std::vector<Road *> AutoRoadContact::GetRelaRoad(OpenDrive *pxodr, Road *pRoad)
{
    std::vector<Road *> xvectorRoadRela;
    unsigned int i;
    if(pRoad->GetPredecessor() != 0)
    {
        if(pRoad->GetPredecessor()->GetElementType() == "road")
        {
            std::string roadid = pRoad->GetPredecessor()->GetElementId();
            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
            if(pRoadTem != 0)
            {
                xvectorRoadRela.push_back(pRoadTem);
            }
        }
        if(pRoad->GetPredecessor()->GetElementType() == "junction")
        {
            std::string junctionid = pRoad->GetPredecessor()->GetElementId();
            Junction * pJunction = xodrfunc::GetJunctionByID(pxodr,junctionid);
            if(pJunction != 0)
            {
                unsigned int njucntionconcount = pJunction->GetJunctionConnectionCount();
                for(i=0;i<njucntionconcount;i++)
                {
                    JunctionConnection * pJC = pJunction->GetJunctionConnection(i);
                    if(pJC == NULL)continue;
                    if(pJC->GetIncomingRoad() == pRoad->GetRoadId())
                    {
                        std::string roadid = pJC->GetConnectingRoad();
                        Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
                        if(pRoadTem != 0)
                        {
                            xvectorRoadRela.push_back(pRoadTem);
                        }
                    }
                    else
                    {
                        if(pJC->GetConnectingRoad() == pRoad->GetRoadId())
                        {
                            std::string roadid = pJC->GetIncomingRoad();
                            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
                            if(pRoadTem != 0)
                            {
                                xvectorRoadRela.push_back(pRoadTem);
                            }
                        }
                    }
                }
            }
        }
    }
    if(pRoad->GetSuccessor() != 0)
    {
        if(pRoad->GetSuccessor()->GetElementType() == "road")
        {
            std::string roadid = pRoad->GetSuccessor()->GetElementId();
            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
            if(pRoadTem != 0)
            {
                xvectorRoadRela.push_back(pRoadTem);
            }
        }
        if(pRoad->GetSuccessor()->GetElementType() == "junction")
        {
            std::string junctionid = pRoad->GetSuccessor()->GetElementId();
            Junction * pJunction = xodrfunc::GetJunctionByID(pxodr,junctionid);
            if(pJunction != 0)
            {
                unsigned int njucntionconcount = pJunction->GetJunctionConnectionCount();
                for(i=0;i<njucntionconcount;i++)
                {
                    JunctionConnection * pJC = pJunction->GetJunctionConnection(i);
                    if(pJC == NULL)continue;
                    if(pJC->GetIncomingRoad() == pRoad->GetRoadId())
                    {
                        std::string roadid = pJC->GetConnectingRoad();
                        Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
                        if(pRoadTem != 0)
                        {
                            xvectorRoadRela.push_back(pRoadTem);
                        }
                    }
                    else
                    {
                        if(pJC->GetConnectingRoad() == pRoad->GetRoadId())
                        {
                            std::string roadid = pJC->GetIncomingRoad();
                            Road * pRoadTem = xodrfunc::GetRoadByID(pxodr,roadid);
                            if(pRoadTem != 0)
                            {
                                xvectorRoadRela.push_back(pRoadTem);
                            }
                        }
                    }
                }
            }
        }
    }

    return xvectorRoadRela;


}

bool AutoRoadContact::RoadToRoad(OpenDrive * pxodr,Road * pRoad1,bool bRoad1Start,Road * pRoad2,bool bRoad2Start,std::vector<Road *> & xvectorthroughroad)
{
    RoadLink * pLink;
    if(bRoad1Start == false)
    {
        pLink = pRoad1->GetSuccessor();
    }
    else
    {
        pLink = pRoad1->GetPredecessor();
    }

    if(pLink->GetElementType() == "road")
    {
        std::string strroadid = pLink->GetElementId();
        Road * pcenterroad = xodrfunc::GetRoadByID(pxodr,strroadid);

    }
    return false;
}
bool AutoRoadContact::IsExist(OpenDrive *pxodr, iv::RoadContactUnit xRCU)
{
    Road * pRoad1 = xRCU.mpRoad1;
    Road * pRoad2 = xRCU.mpRoad2;
    std::vector<Road *> xvectorRoad1Rela;
    std::vector<Road *> xvectorRoad2Rela;
    xvectorRoad1Rela = AutoRoadContact::GetRelaRoad(pxodr,pRoad1);
    xvectorRoad2Rela = AutoRoadContact::GetRelaRoad(pxodr,pRoad2);

    if((xRCU.mcontactype ==0)||(xRCU.mcontactype == 1))
    {
        if(pRoad1->GetPredecessor() == NULL)
        {
            return false;
        }
        RoadLink * pLink = pRoad1->GetPredecessor();
        if(pLink->GetElementType() == "road")
        {
            Road * pRoadCenter = xodrfunc::GetRoadByID(pxodr,pLink->GetElementId());
            if(pRoadCenter != 0)
            {
                if(pRoadCenter == pRoad2)
                {
                    return true;
                }
                else
                {
                    if(pLink->GetContactPoint() == "end")
                    {
                        RoadLink * pLinkCenter = pRoadCenter->GetPredecessor();
                        if(pLinkCenter == NULL)
                        {
                            return false;
                        }
                    }
                }
            }
        }
    }

    //Not turn
    if(xRCU.mturnstraight != 0)
    {
        unsigned int i;
        for(i=0;i<xvectorRoad2Rela.size();i++)
        {
            if(xvectorRoad2Rela[i] == pRoad1)
            {
                return true;
            }
        }
        for(i=0;i<xvectorRoad1Rela.size();i++)
        {
            if(xvectorRoad1Rela[i] == pRoad2)
            {
                return true;
            }
        }
    }
    return false;
}

int AutoRoadContact::MakeAllContact(OpenDrive *pxodr, const double fDisTolerance)
{
    unsigned int nRoadCount = pxodr->GetRoadCount();
    int i;
    std::vector<iv::RoadContactUnit> xvectorRCU;
    for(i=0;i<nRoadCount;i++)
    {
        Road * pnowRoad = pxodr->GetRoad(i);
        unsigned int j;
        for(j=0;j<nRoadCount;j++)
        {
            if(j != i)
            {
                Road * potherRoad = pxodr->GetRoad(j);
                int contactype;
                int turnstraight;
                std::vector<iv::ARC> xARCLane,xARCOpLane;
                int nARCRtn = CalcContact(pnowRoad,potherRoad,contactype,turnstraight,xARCLane,xARCOpLane);
                if(nARCRtn == 0)
                {
                    std::cout<<" Road:"<<pnowRoad->GetRoadId()<<" Road:"<<potherRoad->GetRoadId()
                            <<" contact type: "<<turnstraight<<std::endl;

                    if(xARCLane.size()>0 || (xARCOpLane.size()>0))
                    {
                        iv::RoadContactUnit rcu;
                        rcu.mARCLane = xARCLane;
                        rcu.mARCOpLane = xARCOpLane;
                        rcu.mcontactype = contactype;
                        rcu.mturnstraight = turnstraight;
                        rcu.mpRoad1 = pnowRoad;
                        rcu.mpRoad2 = potherRoad;
                        xvectorRCU.push_back(rcu);
                    }
                }
            }
        }
    }

    xodrdijkstra * pxodrdj = new xodrdijkstra(pxodr);

    //Delete Repeate strait
    for(i=0;i<xvectorRCU.size();i++)
    {
        unsigned int j;
        for(j=(i+1);j<xvectorRCU.size();j++)
        {
            if((xvectorRCU[i].mpRoad1 == xvectorRCU[j].mpRoad2)&&(xvectorRCU[i].mpRoad2 == xvectorRCU[j].mpRoad1))
            {
                if((xvectorRCU[i].mturnstraight == 1)&&(xvectorRCU[j].mturnstraight == 1))
                {
                    std::cout<<"because repeate straight.erase Road:"<<xvectorRCU[j].mpRoad1->GetRoadId()
                            <<" Road:"<<xvectorRCU[j].mpRoad2->GetRoadId()
                            <<" contact type: "<<xvectorRCU[j].mturnstraight<<std::endl;
                    xvectorRCU.erase(xvectorRCU.begin()+j);
                    j = j-1;
                }
            }
        }
    }

    int RCUSize = xvectorRCU.size();
    for(i=0;i<xvectorRCU.size();i++)
    {
        int nlr1 = 0;
        int nlr2 = 0;
        if((xvectorRCU[i].mcontactype == 0)||(xvectorRCU[i].mcontactype == 1))
        {
            nlr1 = 1;
        }
        else
        {
            nlr1 = 2;
        }

        if((xvectorRCU[i].mcontactype == 1)||(xvectorRCU[i].mcontactype == 3))
        {
            nlr2 = 1;
        }
        else
        {
            nlr2 = 2;
        }

        bool bPath;
        Road * pRoad1 = xvectorRCU[i].mpRoad1;
        Road * pRoad2 = xvectorRCU[i].mpRoad2;
        std::vector<int> xvectorpath = pxodrdj->getpath(atoi(pRoad1->GetRoadId().data()),nlr1,
                         atoi(pRoad2->GetRoadId().data()),nlr2,
                         bPath);
        if(bPath)
        {
            if((xvectorpath.size()==2)||(xvectorpath.size() == 3))
            {
               std::vector<pathsection> xvp = pxodrdj->getgpspoint(atoi(pRoad1->GetRoadId().data()),nlr1,
                                                                   atoi(pRoad2->GetRoadId().data()),nlr2,
                                                                   xvectorpath,1);
                unsigned int j;
                std::cout<<"path have ";
                for(j=0;j<xvp.size();j++)
                {
                    std::cout<<j<<": "<<xvp[j].mroadid<<" ";
                }
                std::cout<<std::endl;
                xvectorRCU.erase(xvectorRCU.begin()+i);
                i = i-1;

                    if(xvp.size() == 3)
                    {
                        int nroadid = xvp[1].mroadid;
                        std::cout<<" found junction road . id: "<<nroadid<<std::endl;
                        int k = 0;
                        bool bdel = false;
                        for(k=0;k<xvectorRCU.size();k++)
                        {
                            if((atoi(xvectorRCU[k].mpRoad1->GetRoadId().data()) == nroadid)||(atoi(xvectorRCU[k].mpRoad2->GetRoadId().data()) == nroadid))
                            {
                               xvectorRCU.erase(xvectorRCU.begin() + k);
                               k = k-1;
                               bdel = true;
                            }
                        }
                        if(bdel)
                        {
                            i = -1;
                        }

                    }

            }
        }

    }

    std::cout<<" RCU Size is "<<xvectorRCU.size()<<std::endl;

//    for(i=0;i<xvectorRCU.size();i++)
    std::vector<std::vector<iv::roadcontact>> xvectorrcs;
    for(i=0;i<xvectorRCU.size();i++)
    {
        std::vector<iv::roadcontact> xvectorrc;
        xvectorrc.clear();
        iv::roadcontact rc;
        if((xvectorRCU[i].mcontactype == 0) ||(xvectorRCU[i].mcontactype == 1))
        {
            rc.mncon1 = 0;
        }
        else
        {
            rc.mncon1 = 1;
        }
        if((xvectorRCU[i].mcontactype == 0) ||(xvectorRCU[i].mcontactype == 2))
        {
            rc.mncon2 = 0;
        }
        else
        {
            rc.mncon2 = 1;
        }
        rc.mnroad1id = atoi(xvectorRCU[i].mpRoad1->GetRoadId().data());
        rc.mnroad2id = atoi(xvectorRCU[i].mpRoad2->GetRoadId().data());
        unsigned int j;
        for(j=0;j<xvectorRCU[i].mARCLane.size();j++)
        {
             iv::lanecontact lt;
             lt.ml1 = xvectorRCU[i].mARCLane[j].from;
             lt.ml2 = xvectorRCU[i].mARCLane[j].to;
             rc.mvectorlc.push_back(lt);
        }
        for(j=0;j<xvectorRCU[i].mARCOpLane.size();j++)
        {
             iv::lanecontact lt;
             lt.ml1 = xvectorRCU[i].mARCOpLane[j].from;
             lt.ml2 = xvectorRCU[i].mARCOpLane[j].to;
             rc.mvectorlcop.push_back(lt);
        }
        xvectorrc.push_back(rc);
        xvectorrcs.push_back(xvectorrc);
//        CreateContactRoad(xvectorrc,pxodr,xvectorRCU[i].mturnstraight);
    }

    for(i=0;i<xvectorrcs.size();i++)
    {
        CreateContactRoad(xvectorrcs[i],pxodr,xvectorRCU[i].mturnstraight);
    }

    return 0;
}

void AutoRoadContact::CreateContactRoad(std::vector<iv::roadcontact> & xvectorrc,OpenDrive * pxodr,int nConType)
{
    if(xvectorrc.size()<1)return;

    Road * p1, *p2;
    int nroad1index;
    int nroad2index;

    int i;
    bool bhavep1 = false;
    bool bhavep2 = false;
    for(i=0;i<pxodr->GetRoadCount();i++)
    {
        if(xvectorrc[0].mnroad1id == atoi(pxodr->GetRoad(i)->GetRoadId().data()))
        {
            bhavep1 = true;
            p1 = pxodr->GetRoad(i);
            nroad1index = i;
            break;
        }
    }

    if(bhavep1 == false)
    {
        std::cout<<"Road not found"<<std::endl;
        return;
    }

    double off1,off2;



    for(i=0;i<pxodr->GetRoadCount();i++)
    {
        if(xvectorrc[0].mnroad2id == atoi(pxodr->GetRoad(i)->GetRoadId().data()))
        {
            bhavep2 = true;
            p2 = pxodr->GetRoad(i);
            nroad2index = i;
            break;
        }
    }

    if(bhavep2 == false)
    {
        std::cout<<"Road not found"<<std::endl;
        return;
    }

    if(xvectorrc[0].mvectorlc.size()<1)
    {
        std::cout<<"No Lane Contact."<<std::endl;;
        return;
    }

    double startx,starty,starthdg;
    double endx,endy,endhdg;

    double startheight,endheight;

    bool bFromstart,bTostart;
    if(xvectorrc[0].mncon1 == 0)
    {
        bFromstart = true;
        starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
        off1 = MainWindow::getoff(p1,xvectorrc[0].mvectorlc[0].ml1,true);
        startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
        starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();

        if(p1->GetLaneOffsetCount()>0)
        {
            off1 = off1 - p1->GetLaneOffset(0)->Geta();
        }
        startx = startx + off1 * cos(starthdg -M_PI/2.0);
        starty = starty + off1 * sin(starthdg -M_PI/2.0);

        startheight = 0;
        if(p1->GetElevationCount()>0)
        {
            startheight = p1->GetElevation(0)->GetA();
        }

 //       if(mvectorrc[0].mvectorlc[0].ml1<0)
            starthdg = starthdg +M_PI;if(starthdg >=2.0*M_PI)starthdg = starthdg -2.0*M_PI;
    }
    else
    {
        bFromstart = false;
        if(GetEndPoint(p1,startx,starty,starthdg) != 0)
        {
            std::cout<<"Get End Point Error."<<std::endl;
            return;
        }
        off1 = MainWindow::getoff(p1,xvectorrc[0].mvectorlc[0].ml1,false);
        if(p1->GetLaneOffsetCount()>0)
        {
            LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);
            double froadlen = p1->GetRoadLength();
            double sdis = froadlen - pLO->GetS();
            double foffset = pLO->Geta() + pLO->Getb()*(sdis) + pLO->Getc() * sdis * sdis
                    +pLO->Getd() * sdis * sdis * sdis;
            off1 = off1 - foffset;
        }
        startx = startx + off1 * cos(starthdg -M_PI/2.0);
        starty = starty + off1 * sin(starthdg -M_PI/2.0);



        startheight = 0;
        if(p1->GetElevationCount()>0)
        {
            startheight = p1->GetElevation(0)->GetA()
                    +p1->GetElevation(0)->GetB() * pow(p1->GetRoadLength(),1)
                    +p1->GetElevation(0)->GetC() * pow(p1->GetRoadLength(),2)
                    +p1->GetElevation(0)->GetD() * pow(p1->GetRoadLength(),3);
        }
    }

    if(xvectorrc[0].mncon2 == 0)
    {
        bTostart = true;
        off2 = MainWindow::getoff(p2,xvectorrc[0].mvectorlc[0].ml2,true);
        endx = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
        endy = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
        endhdg = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();

        if(p2->GetLaneOffsetCount()>0)
        {
            off2 = off2 - p2->GetLaneOffset(0)->Geta();
        }
        endx = endx + off2 * cos(endhdg -M_PI/2.0);
        endy = endy + off2 * sin(endhdg -M_PI/2.0);

        endheight = 0;
        if(p2->GetElevationCount()>0)
        {
            endheight = p2->GetElevation(0)->GetA();
        }
    }
    else
    {
        bTostart = false;
        off2 = MainWindow::getoff(p2,xvectorrc[0].mvectorlc[0].ml2,false);
        if(GetEndPoint(p2,endx,endy,endhdg) != 0)
        {
            std::cout<<"get end error."<<std::endl;
            return;
        }
        if(p2->GetLaneOffsetCount()>0)
        {
            LaneOffset * pLO = p2->GetLaneOffset(p2->GetLaneOffsetCount()-1);
            double froadlen = p2->GetRoadLength();
            double sdis = froadlen - pLO->GetS();
            double foffset = pLO->Geta() + pLO->Getb()*(sdis) + pLO->Getc() * sdis * sdis
                    +pLO->Getd() * sdis * sdis * sdis;
            off2 = off2 - foffset;
        }
        endx = endx + off2 * cos(endhdg -M_PI/2.0);
        endy = endy + off2 * sin(endhdg -M_PI/2.0);
        endhdg = endhdg +M_PI;if(endhdg >=2.0*M_PI)endhdg = endhdg -2.0*M_PI;

        endheight = 0;
        if(p2->GetElevationCount()>0)
        {
            endheight = p2->GetElevation(0)->GetA()
                    +p2->GetElevation(0)->GetB() * pow(p2->GetRoadLength(),1)
                    +p2->GetElevation(0)->GetC() * pow(p2->GetRoadLength(),2)
                    +p2->GetElevation(0)->GetD() * pow(p2->GetRoadLength(),3);
        }
    }


    //Create Geo
    double R = 6.0;
    std::vector<geobase> xvectorgeo;
    std::vector<geobase> xvectorgeo1,xvectorgeo2;
    switch(nConType)
    {
    case 0:
        xvectorgeo = geofit::CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
        break;
    case 1:
        xvectorgeo = geofit::CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
        break;
    case 2:
        xvectorgeo = geofit::CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
        break;
    default:
        break;
    }

    if(nConType == 2)
    {
        for(i=0;i<xvectorgeo.size()/2;i++)
        {
            xvectorgeo1.push_back(xvectorgeo.at(i));
        }
        for(i=xvectorgeo.size()/2;i<xvectorgeo.size();i++)
        {
            xvectorgeo2.push_back(xvectorgeo.at(i));
        }
    }

    if(xvectorgeo.size() == 0)
    {
        std::cout<<"Waring Create Road Fail."<<std::endl;
        return;
    }

    double xroadlen = 0;

    if(nConType != 2)
    {
        for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;

        pxodr->AddRoad("",xroadlen, QString::number(gw->CreateRoadID()).toStdString(),"-1");
        Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);

        p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);

        p1 = pxodr->GetRoad(nroad1index);
        p2 = pxodr->GetRoad(nroad2index);

        double s = 0;
        int j;
        j= 0;
        for(j=0;j<xvectorgeo.size();j++)
        {
            p->AddGeometryBlock();
            GeometryBlock * pgb = p->GetGeometryBlock(j);

            geobase * pline;
            geobase * pbez;
            geobase * parc;
            switch(xvectorgeo[j].mnType)
            {
            case 0:
                pline = &xvectorgeo[j];
                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
                break;
            case 1:
                parc = &xvectorgeo[j];
                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
                break;
            case 2:
                pbez = &xvectorgeo[j];
                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
                break;
            }
            s = s + xvectorgeo[j].mfLen;
        }

        p->AddLaneSection(0);
        LaneSection * pLS = p->GetLaneSection(0);
        pLS->SetS(0);
        pLS->AddLane(0,0,"none",false);

        double * pswidth,*pewidth;
        std::vector<std::string> strvectorlanetype;
        int nlanecount = xvectorrc[0].mvectorlc.size();
        pswidth = new double[nlanecount];
        pewidth = new double[nlanecount];
        std::shared_ptr<double> ppswidth,ppewidth;
        ppswidth.reset(pswidth);
        ppewidth.reset(pewidth);

        for(i=0;i<nlanecount;i++)
        {
            pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
            strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart));

        }
        for(i=0;i<nlanecount;i++)
        {
            pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlc.at(i).ml2,bTostart);
        }

        double * pa,*pb;
        pa = new double[nlanecount];
        pb = new double[nlanecount];
        std::shared_ptr<double> ppa,ppb;
        ppa.reset(pa);
        ppb.reset(pb);

        for(i=0;i<nlanecount;i++)
        {
            pa[i] = pswidth[i];
            pb[i] = (pewidth[i] - pa[i])/xroadlen;
        }

        for(i=0;i<nlanecount;i++)
        {
            pLS->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
            Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);

            pLL->AddWidthRecord(0,pa[i],pb[i],
                                0,0);
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

        }

        int noplanecount = xvectorrc[0].mvectorlcop.size();
        if(noplanecount > 0)
        {
            pswidth = new double[noplanecount];
            pewidth = new double[noplanecount];
            ppswidth.reset(pswidth);
            ppewidth.reset(pewidth);

            strvectorlanetype.clear();
            for(i=0;i<noplanecount;i++)
            {
                pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
                strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
            }
            for(i=0;i<noplanecount;i++)
            {
                pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
            }

            pa = new double[noplanecount];
            pb = new double[noplanecount];
            ppa.reset(pa);
            ppb.reset(pb);

            for(i=0;i<noplanecount;i++)
            {
                pa[i] = pswidth[i];
                pb[i] = (pewidth[i] - pa[i])/xroadlen;
            }

            for(i=0;i<noplanecount;i++)
            {
                pLS->AddLane(1,(i+1),strvectorlanetype[i],false,false);
                Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);

                pLL->AddWidthRecord(0,pa[i],pb[i],
                                    0,0);
                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

            }
        }

    }
    else
    {
        double xroadlen1 = 0;
        double xroadlen2 = 0;
        for(i=0;i<xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
        for(i=0;i<xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;

        int index1 = pxodr->AddRoad("",xroadlen1, QString::number(gw->CreateRoadID()).toStdString(),"-1");
        int index2 = pxodr->AddRoad("",xroadlen2, QString::number(gw->CreateRoadID()).toStdString(),"-1");
        Road * proad2 = pxodr->GetRoad(index2);
        Road * proad1 = pxodr->GetRoad(index1);

        proad1->AddElevation(0,startheight,(endheight-startheight)/(xroadlen1+xroadlen2),0,0);
        proad2->AddElevation(0,startheight+xroadlen1*(endheight-startheight)/(xroadlen1+xroadlen2),
                             (endheight-startheight)/(xroadlen1+xroadlen2),
                             0,0);

        p1 = pxodr->GetRoad(nroad1index);
        p2 = pxodr->GetRoad(nroad2index);

//        OpenDrive * px = &mxodr;
        double s = 0;
        int j;
        j= 0;
        for(j=0;j<xvectorgeo1.size();j++)
        {
            proad1->AddGeometryBlock();
            GeometryBlock * pgb = proad1->GetGeometryBlock(j);

            geobase * pline;
            geobase * pbez;
            geobase * parc;
            switch(xvectorgeo1[j].mnType)
            {
            case 0:
                pline = &xvectorgeo1[j];
                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
                break;
            case 1:
                parc = &xvectorgeo1[j];
                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
                break;
            case 2:
                pbez = &xvectorgeo1[j];
                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
                break;
            }
            s = s + xvectorgeo1[j].mfLen;
        }

        s=0.0;
        for(j=0;j<xvectorgeo2.size();j++)
        {
            proad2->AddGeometryBlock();
            GeometryBlock * pgb = proad2->GetGeometryBlock(j);

            geobase * pline;
            geobase * pbez;
            geobase * parc;
            switch(xvectorgeo2[j].mnType)
            {
            case 0:
                pline = &xvectorgeo2[j];
                pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
                break;
            case 1:
                parc = &xvectorgeo2[j];
                pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
                break;
            case 2:
                pbez = &xvectorgeo2[j];
                std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
                pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
                                           pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
                        pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
                        pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
                break;
            }
            s = s + xvectorgeo2[j].mfLen;
        }

        proad1->AddLaneSection(0);
        LaneSection * pLS1 = proad1->GetLaneSection(0);
        pLS1->SetS(0);
        pLS1->AddLane(0,0,"none",false);

        proad2->AddLaneSection(0);
        LaneSection * pLS2 = proad2->GetLaneSection(0);
        pLS2->SetS(0);
        pLS2->AddLane(0,0,"none",false);

        double * pswidth,*pewidth;
        int nlanecount = xvectorrc[0].mvectorlc.size();
        std::vector<std::string> strvectorlanetype;
        pswidth = new double[nlanecount];
        pewidth = new double[nlanecount];
        std::shared_ptr<double> ppswidth,ppewidth;
        ppswidth.reset(pswidth);
        ppewidth.reset(pewidth);

        for(i=0;i<nlanecount;i++)
        {
            pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
            strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlc.at(i).ml1,bFromstart));

        }
        for(i=0;i<nlanecount;i++)
        {
            pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlc.at(i).ml2,bTostart);
        }

        double * pa,*pb;
        pa = new double[nlanecount];
        pb = new double[nlanecount];
        std::shared_ptr<double> ppa,ppb;
        ppa.reset(pa);
        ppb.reset(pb);

        for(i=0;i<nlanecount;i++)
        {
            pa[i] = pswidth[i];
            pb[i] = (pewidth[i] - pa[i])/(xroadlen1+xroadlen2);
        }

        for(i=0;i<nlanecount;i++)
        {
            pLS1->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
            Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);

            pLL->AddWidthRecord(0,pa[i],pb[i],
                                0,0);
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

            pLS2->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
            pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);

            pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
                                0,0);
            pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

        }
        int noplanecount = xvectorrc[0].mvectorlcop.size();
        if(noplanecount > 0)
        {
            pswidth = new double[noplanecount];
            pewidth = new double[noplanecount];
            ppswidth.reset(pswidth);
            ppewidth.reset(pewidth);

            strvectorlanetype.clear();
            for(i=0;i<noplanecount;i++)
            {
                pswidth[i] =  gw->getlanewidth(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
                strvectorlanetype.push_back(gw->getlanetype(p1,xvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
            }
            for(i=0;i<noplanecount;i++)
            {
                pewidth[i] =  gw->getlanewidth(p2,xvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
            }

            pa = new double[noplanecount];
            pb = new double[noplanecount];
            ppa.reset(pa);
            ppb.reset(pb);

            for(i=0;i<noplanecount;i++)
            {
                pa[i] = pswidth[i];
                pb[i] = (pewidth[i] - pa[i])/xroadlen;
            }

            for(i=0;i<noplanecount;i++)
            {
                pLS1->AddLane(1,(i+1),strvectorlanetype[i],false,false);
                Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);

                pLL->AddWidthRecord(0,pa[i],pb[i],
                                    0,0);
                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

                pLS2->AddLane(1,(i+1),strvectorlanetype[i],false,false);
                pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);

                pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
                                    0,0);
                pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");

            }
        }


    }


}