/* #########################################
 * Traffic monitoring using Raspberry PI
 * Author: Michal Zacpal
 * Library: Class RoadLane - Separation & Recognition RoadLane
 * 2015
 * ######################################### */
#include "roadlane.h"
//Constructors
RoadLane::RoadLane(LaneCoordinates _LaneCoor,int iImageHeight)
{
    Length = 0;
    Height = 0;
    dCoeff = 0.1;
    NoLane = false;
    Desc1 = 0;
    NumberLane = 0;
    Type = -1;
    if (!_LaneCoor.L.empty()){
        SeparateLane(_LaneCoor,dCoeff, iImageHeight);
        LineCenter();
    }
    else
        NoLane = true;
    if (!Lanes.empty())
        RecognizeLane(iImageHeight);
    else
        NoLane = true;
}
RoadLane::RoadLane(){
    Length = 0;
    Height = 0;
    NoLane = false;
    Desc1 = 0;
    dCoeff = 0.1;
    Type = -1;
    NoLane = true;
    NumberLane = 0;
}
RoadLane::RoadLane( const RoadLane &obj){
    Length = obj.Length;
    Height = obj.Height;
    dCoeff = obj.dCoeff;
    NoLane = obj.NoLane;
    Desc1 = obj.Desc1;
    Desc2 = obj.Desc2;
    Type = obj.Type;       // 0 - solid; 1 - Dashed; 2 - DashedConnect
    NumberLane = obj.NumberLane;
    Lanes = obj.Lanes;
    CenterOfLine = obj.CenterOfLine;

}
//Destructor
RoadLane::~RoadLane()
{}
// Methods
/*
 *Separate lane from Coordinates of lane
 */
void RoadLane::SeparateLane(LaneCoordinates _LaneCoor,double dCoeff, int iImageHeight)
{
    if (_LaneCoor.L.empty())
    {
        return;
    }
    int iLine = 0;
    for (int i = 0; i < _LaneCoor.L.size(); i++)
    {
        if ((_LaneCoor.L[i+1].y -  _LaneCoor.L[i].y) == 5)
        {
            iLine++;
        }
        else
        {
            if (((double)(5*iLine)/(double)iImageHeight) > dCoeff)
            {
                LaneCoordinates NewLane;
                for (int j = i-iLine; j < i; j++)
                {
                    NewLane.L.push_back(_LaneCoor.L[j]);
                    NewLane.R .push_back(_LaneCoor.R[j]);
                }
                Lanes.push_back(NewLane);
                iLine = 0;
                NumberLane++;
            }
            else{
                iLine = 0;
            }
        }
    }
}
/*
 * Recognition of Road lane from separated lane
 */
void RoadLane::RecognizeLane(int iImageHeight)
{
    //iImageHeight = iImageHeight - 10;
    for (int i = 0; i < NumberLane; i++)
    {
        Desc1 = Desc1 + (double) (CenterOfLine[i].size() * 5) / (double) iImageHeight;
    }
    int endLine = 0;
    Desc2 = CenterOfLine[0][0].y;
    if (NumberLane > 1){
        for(int i = 0; i < NumberLane-1; i++){
            endLine  = CenterOfLine[i].size()-1;
            int e = CenterOfLine[i+1][0].y;
            int b = CenterOfLine[i][endLine].y;
            Desc2 = Desc2 + e-b;
        }
    }
    endLine  = CenterOfLine[NumberLane-1].size()-1;
    Desc2 = Desc2 + iImageHeight - CenterOfLine[NumberLane-1][endLine].y;
    Desc2 = Desc2 / (double)iImageHeight;
    Desc1 = Desc1 / NumberLane;
    if (Desc1 < dCoeffDashCon1 && Desc2 <= dCoeffDashCon2 && Desc2 > dCoeffSolid2)
        Type = 2;       // Dashed connect
    if (Desc1 >= dCoeffDashCon1 && Desc1 < dCoeffSolid1 &&  Desc2 >= dCoeffDash2)
        Type = 1;       // Dashed
    if (Desc1 >= dCoeffSolid1 && Desc2 <= dCoeffSolid2)
        Type = 0;       // Solid
}
/*
 * Compute Coordinates of line center from LaneCoordinates
 */
void RoadLane::LineCenter()
{
    if (Lanes.empty())
        return;
    for (int i = 0; i < Lanes.size(); i++)
    {
        vector<Point>  NewVector;
        for (int j = 0; j < Lanes[i].L.size(); j++)
        {
            int iCoorX = ((Lanes[i].R[j].x - Lanes[i].L[j].x) /2) + Lanes[i].L[j].x;
            NewVector.push_back(Point(iCoorX,Lanes[i].L[j].y));
        }
        CenterOfLine.push_back(NewVector);
    }

}
