# ##### BEGIN GPL LICENSE BLOCK #####
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#   
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software Foundation,
#  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#
# ##### END GPL LICENSE BLOCK #####


import struct


def encodeASCIILine(line):
    return line.decode(encoding='ASCII')



class Point:

    def __init__(self):
        pass


    def setField(self, fieldname, values):
        pass




class PointXYZ(Point):

    x = 0
    y = 0
    z = 0

    def __init__(self):
        super().__init__()


    def setField(self, fieldname, values):
        value = values[0]
        if fieldname == 'x':
            self.x = value
        elif fieldname == 'y':
            self.y = value
        elif fieldname == 'z':
            self.z = value




class PCDParser:

    filepath = ''
    file = None

    points = []
    PointClass = None

    headerEnd = False


    @staticmethod
    def factory(filepath, PointClass):
        version = 'NO_VERSION_NUMBER'
        with open(filepath, 'rb') as f:
            for b in f:
                line = encodeASCIILine(b)
                line_split = line.split()
                if line_split[0] == 'VERSION' and len(line_split) > 1:
                    version = line_split[1]
                    break
      
        if version == ".7" or version == "0.7": 
            return PCDParser_v0_7(filepath, PointClass)
        else:
            return None


    def __init__(self, filepath, PointClass):
        self.filepath = filepath
        self.PointClass = PointClass

        self.file = None
        self.headerEnd = False
        self.points = []


    def parserWarning(self, msg):
        print("[WARNING] ", msg)
    

    def rmComment(self, line):
        return line[:line.find('#')] 


    def parseFile(self):
        with open(self.filepath, 'rb') as self.file:
            self.parseHeader()
            self.parsePoints()


    def parseHeader(self):
        for b in self.file:
            line = encodeASCIILine(b)
            line = self.rmComment(line)

            split = line.split()
            if len(split) > 0:
                self.parseHeaderLine(split)

            if self.headerEnd:
                self.finalizeHeader()
                break


    def parseHeaderLine(self, split):
        keyword = split[0]
        self.parserWarning("Uknown header Keyword '" + keyword + "' gets ignored")

    
    def finalizeHeader(self):
        pass


    def parsePoints(self):
        pass


    def getPoints(self):
        return self.points


    def version(self):
        return 'NO_VERSION_NUMBER'




class PCDParser_v0_7(PCDParser):

    fields = []

    def __init__(self, filepath, PointClass):
        super().__init__(filepath, PointClass)
        self.fields = []


    def version(self):
        return '.7'


    def parseHeaderLine(self, split):
        keyword = split[0]
        if keyword == 'VERSION':
            self.parseVERSION(split[1:])
        elif keyword == 'FIELDS':
            self.parseFIELDS(split[1:])
        elif keyword == 'SIZE':
            self.parseSIZE(split[1:])
        elif keyword == 'TYPE':
            self.parseTYPE(split[1:])
        elif keyword == 'COUNT':
            self.parseCOUNT(split[1:])
        elif keyword == 'WIDTH':
            self.parseWIDTH(split[1:])
        elif keyword == 'HEIGHT':
            self.parseHEIGHT(split[1:])
        elif keyword == 'POINTS':
            self.parsePOINTS(split[1:])
        elif keyword == 'DATA':
            self.parseDATA(split[1:])
        else:
            super().parseHeaderLine(split)


    def parseVERSION(self, split):
        pass


    def parseFIELDS(self, split):
        print("SPLIT FIELDS:", split)
        for field in split:
            self.fields.append([field, None, None, None])
        print("FIELDS, after parsing:", self.fields)


    def parseSIZE(self, split):
        for i, size in enumerate(split):
            self.fields[i][1] = int(size)


    def parseTYPE(self, split):
        for i, type in enumerate(split):
            self.fields[i][2] = type


    def parseCOUNT(self, split):
        for i, count in enumerate(split):
            self.fields[i][3] = int(count)


    def parseWIDTH(self, split):
        self.width = int(split[0])


    def parseHEIGHT(self, split):
        self.height = int(split[0])


    def parsePOINTS(self, split):
        pass


    def parseDATA(self, split):
        if split[0] == "ascii":
            self.datatype = 'ASCII'
        elif split[0] == "binary":
            self.datatype = 'BINARY'
        self.headerEnd = True


    def finalizeHeader(self):
        self.numPoints = self.width * self.height
        print("FIELDS - finalized", self.fields)


    def parsePoints(self):
        if self.datatype == 'ASCII':
            self.parseASCII()
        elif self.datatype == 'BINARY':
            self.parseBINARY()


    def parseASCII(self):
        parsedPoints = 0
        while parsedPoints < self.numPoints:
           
            try: 
                b = self.file.readline()
                line = encodeASCIILine(b)
            except:
                self.parserError("Unexpected end of data")
                return
            line = self.rmComment(line)
            split = line.split()

            if (len(split) == 0):
                continue
            else:
                parsedPoints += 1

            point = self.PointClass()

            for field in self.fields:
                fieldname = field[0]
                fieldtype = field[2]
                fieldcount = field[3]

                values = []
                for i in range(fieldcount):
                    vs = split.pop(0)
                    if fieldtype == 'F':
                        values.append(float(vs))
                    elif fieldtype in ['U', 'I']:
                        values.append(int(vs))

                point.setField(fieldname, values)

            self.points.append(point)


    def parseBINARY(self):
        for pointi in range(self.numPoints):
            point = self.PointClass()

            for field in self.fields:
                fieldname = field[0]
                fieldsize = field[1]
                fieldtype = field[2]
                fieldcount = field[3]

                values = []
                for i in range(fieldcount):

                    fs = None
                    if fieldtype == 'F':
                        if fieldsize == 4: #float
                            fs = '<f'
                        elif fieldsize == 8: #double
                            fs = '<d'
                    elif fieldtype == 'U':
                        if fieldsize == 1: #unsinged char
                            fs = '<B'
                        elif fieldsize == 2: #unsinged short
                            fs = '<H'
                        elif fieldsize == 4: #unsinged int
                            fs =  '<I'
                    elif fieldtype == 'I':
                        if fieldsize == 1: #char
                            fs = '<c'
                        elif fieldsize == 2: #short
                            fs = '<h'
                        elif fieldsize == 4: #signed int
                            fs =  '<i'

                    raw = self.file.read(fieldsize)
                    if (fs):
                        data = struct.unpack(fs, raw)
                        values.append(data[0])

                point.setField(fieldname, values)

            self.points.append(point)




def test():
    parser = PCDParser.factory('test.pcd', PointXYZ)
    if parser:
        parser.parseFile()
        points = parser.getPoints()
        for point in points:
            print(point.x, point.y, point.z)
    else:
        print("Can't create parser for this file")

