import math
import json


class RoadNetwork(object):
    def __init__(self, rnd_file, is_json=True):
        """Instantiate road network description

        rnd_file can be string (file name) or file-like object.

        Note that two containers are recognized: JSON and
        plaintext. If the argument is_json=True (default) the file is
        parsed as JSON. Otherwise, the plaintext format is used.
        """
        if getattr(rnd_file, 'readline', None) is None:
            rnd_file = open(rnd_file, 'rt')

        self.version = None
        self.length = None
        self.segments = list()
        self.transform = (0, 0, 0)
        self.shape = None

        if is_json:
            rndjson =json.load(rnd_file)
            self.version = rndjson['version']
            assert self.version == 0, 'Unrecognized format version'
            self.length = rndjson['length']
            self.transform = tuple([float(x) for x in rndjson['transform']])
            if 'segments' in rndjson:
                self.segments = rndjson['segments']
            if 'shape' in rndjson:
                self.shape = tuple(rndjson['shape'])

        else:
            reading_segments = False
            for line in rnd_file:
                line = line.strip()
                if len(line) == 0 or line.startswith('#'):
                    continue  # Ignore comments, blank lines
                comment_index = line.find('#')
                if comment_index > 0:
                    line = line[:comment_index]  # Strip trailing comments
                if not reading_segments:
                    if line.startswith('version'):
                        self.version = int(line.split(':')[1])
                        assert self.version == 0, 'Unrecognized format version'
                    elif line.startswith('length'):
                        self.length = float(line.split(':')[1])
                    elif line.startswith('transform'):
                        self.transform = tuple([float(x) for x in line.split(':')[1].split()])
                    elif line.startswith('segments'):
                        reading_segments = True
                    elif line.startswith('shape'):
                        self.shape = tuple([float(x) for x in line.split(':')[1].split()])
                    else:
                        raise ValueError('Unrecognized file entry.')
                else:
                    self.segments.append([int(x) for x in line.split()])

        assert self.length > 0, 'Unit side length must be positive'
        assert len(self.transform) == 3, 'Map transform must be a 3-tuple'
        if self.shape is not None:
            assert len(self.shape) == 2, 'Shape must be a 2-tuple'
            assert self.shape[0] >= 1 and self.shape[1] >= 1, 'Shape must have positive elements'
        if len(self.segments) > 0:
            segments_shape = [0, 4]
            # extrema elements: x_min, x_max, y_min, y_max
            extrema = [self.segments[0][0], self.segments[0][1],
                       self.segments[0][0], self.segments[0][1]]
            for segment in self.segments:
                assert len(segment) == segments_shape[1], 'Mismatch in row length of segments'
                segments_shape[0] += 1
                extrema[0] = min(segment[0], segment[2], extrema[0])
                extrema[1] = max(segment[0], segment[2], extrema[1])
                extrema[2] = min(segment[1], segment[3], extrema[2])
                extrema[3] = max(segment[1], segment[3], extrema[3])
            if self.shape is None:
                self.shape = (extrema[3] - extrema[2],
                              extrema[1] - extrema[0])
            else:
                assert self.shape == (extrema[3] - extrema[2]+1,
                                      extrema[1] - extrema[0]+1), 'Shape and segments fields are not consistent'
        elif self.shape is not None:
            self.segments = []
            for x in range(self.shape[1]-1):
                for y in range(self.shape[0]-1):
                    self.segments.append([x, y, x+1, y])
                    self.segments.append([x, y, x, y+1])
            for x in range(self.shape[1]-1):
                self.segments.append([x, self.shape[0]-1,
                                      x+1, self.shape[0]-1])
            for y in range(self.shape[0]-1):
                self.segments.append([self.shape[1]-1, y,
                                      self.shape[1]-1, y+1])
        else:
            raise ValueError('At least one of segments or shape must be defined')

    def number_of_segments(self):
        return len(self.segments)

    def get_mapped_segment(self, index):
        if index < 0 or index > len(self.segments)-1:
            raise ValueError('index out of bounds [0,'
                             +str(len(self.segments)-1)+']: '
                             +str(index))
        mapped_segment = [self.segments[index][0]*self.length, self.segments[index][1]*self.length,
                          self.segments[index][2]*self.length, self.segments[index][3]*self.length]
        costheta = math.cos(self.transform[2])
        sintheta = math.sin(self.transform[2])
        for offset in [0, 2]:
            new_x = costheta*mapped_segment[offset] - sintheta*mapped_segment[offset+1]
            new_y = sintheta*mapped_segment[offset] + costheta*mapped_segment[offset+1]
            mapped_segment[offset] = new_x + self.transform[0]
            mapped_segment[offset+1] = new_y + self.transform[1]
        return tuple(mapped_segment)


def road_segment(x1, x2, prefix='straightroad'):
    assert len(x1) == 2 and len(x2) == 2

    center = ((x1[0] + x2[0])/2.0, (x1[1] + x2[1])/2.0)
    length = math.sqrt((x1[0]-x2[0])**2 + (x1[1]-x2[1])**2)
    angle = math.atan2(x2[1]-x1[1], x2[0]-x1[0])
    nl = '\n'
    idt = ' '*2
    output = '<model name="'+prefix+'">'+nl
    output += '<static>true</static>'+nl
    output += '<link name="'+prefix+'link1">'+nl
    output += '<visual name="'+prefix+'bar1">'+nl
    output += '<pose>{X} {Y} 0 0 0 {ANGLE}</pose>'.format(X=center[0], Y=center[1], ANGLE=angle)+nl
    output += '<geometry><box><size>{LENGTH} 1.2 0.001</size></box></geometry>'.format(LENGTH=length)+nl
    output += """
          <material>
	    <ambient>0.2 0.2 0.2 1</ambient>
	    <diffuse>0.2 0.2 0.2 1</diffuse>
	    <specular>0.2 0.2 0.2 1</specular>
	    <emissive>0.2 0.2 0.2 1</emissive>
	  </material>
	</visual>
      </link>
"""
    output += '<link name="'+prefix+'link2">'+nl
    output += '<visual name="'+prefix+'bar2">'+nl
    output += '<pose>{X} {Y} 0 0 0 {ANGLE}</pose>'.format(X=center[0], Y=center[1], ANGLE=angle)+nl
    output += '<geometry><box><size>{LENGTH} 0.12 0.0015</size></box></geometry>'.format(LENGTH=length)+nl
    output += """
          <material>
	    <ambient>0.5 0.5 0 1</ambient>
	    <diffuse>0.5 0.5 0 1</diffuse>
	    <specular>0.5 0.5 0 1</specular>
	    <emissive>0.5 0.5 0 1</emissive>
	  </material>
	</visual>
      </link>
"""
    return output+nl+'</model>'

def gen_worldsdf(roads):
    output = """<?xml version="1.0" ?>
<sdf version="1.4">
  <world name="default">
    <gui><camera name="user_camera"><pose>0 0 9.5 0 1.5 0</pose></camera></gui>

    <include><uri>model://sun</uri></include>
    <include><uri>model://ground_plane</uri></include>
"""
    for sindex in range(roads.number_of_segments()):
        segment = roads.get_mapped_segment(sindex)
        output += road_segment((segment[0], segment[1]),
                               (segment[2], segment[3]),
                               prefix='segment_'+str(sindex)+'_')
    output += '</world></sdf>'
    return output
