#!/usr/bin/env python
# *-----------------------------------------------------------------------*
# |                                                                       |
# |  Copyright (c) 2016 by Paul Scherrer Institute (http://www.psi.ch)    |
# |                                                                       |
# |              Author Thierry Zamofing (thierry.zamofing@psi.ch)        |
# *-----------------------------------------------------------------------*
'''
shape an optimal path with given points

#mode bits:

 0  1 sort and plot random points
 1  2 sort and plot grid(+some random) points
 2  4 generate motion program

verbose bits:
  1 basic info
  2 plot sorting steps
  4 upload progress

#config file example:
{
  "points": [
    [100,523],[635,632],[756,213],
  "mode": ["plot","program","gather","???"],
}

Acquired time is:MaxSamples*Period*.2

'''

import os, sys, json
import numpy as np
import matplotlib as mpl
import matplotlib.pyplot as plt
import subprocess as sprc
import telnetlib

def ConvUtf8(s):
  'convert unicoded json object to ASCII encoded'
  #http://stackoverflow.com/questions/956867/how-to-get-string-objects-instead-of-unicode-ones-from-json-in-python
  if isinstance(s, dict):
    return {ConvUtf8(key): ConvUtf8(value) for key, value in s.iteritems()}
  elif isinstance(s, list):
    return [ConvUtf8(element) for element in s]
  elif isinstance(s, unicode):
    return s.encode('utf-8')
  else:
    return s

class GpasciiCommunicator():
  '''Communicates with the Delta Tau gpascii programm
  '''
  gpascii_ack="\x06\r\n"
  gpascii_inp='Input\r\n'

  def connect(self, host, username='root', password='deltatau',prompt='ppmac# '):
    p=telnetlib.Telnet(host)
    print p.read_until('login: ')
    p.write(username+'\n')
    print p.read_until('Password: ')
    p.write(password+'\n')
    print p.read_until(prompt)  # command prompt
    p.write('gpascii -2\n')  # execute gpascii command
    print p.read_until(self.gpascii_inp)
    return p


class ShapePath:
    def __init__(self,args):
      if args.cfg:
        fh=open(args.cfg,'r')
        s=fh.read()
        cfg=json.loads(s, object_hook=ConvUtf8)
        s=json.dumps(cfg, indent=2, separators=(',', ': '));print s
      else:
        cfg={"points": [[100,523],[635,632],[756,213]],"mode": ["plot","program","gather","???"]}
        #args.cfg={"points": [[100,523],[635,632],[756,213]],"mode": ["plot","program","gather","???"]}
      self.cfg=cfg
      self.args=args

    def run(self):
      print('args='+str(self.args))
      print('cfg='+str(self.cfg))
      mode=self.args.mode
      if mode&1:
        #generate random points and sort
        np.random.seed(0)
        #data=np.random.randint(0,1000,(30,2))
        data=np.random.rand(107,2)*1000
        self.points=data
        self.sort_points()
      if mode&2:
        np.random.seed(0)
        xx,yy=np.meshgrid(range(10), range(10))
        data=np.array([xx.reshape(-1),yy.reshape(-1)],dtype=np.float).transpose()*100
        data+=np.random.rand(100,2)*20
        self.points=data
        self.sort_points()
      if mode&4:
        pass

    def sort_points(self):
      data=self.points
      verb=self.args.verbose
      if verb&2:
        self.plot_points(data)

      #sort points along y
      data=data[data[:, 1].argsort()]
      if verb&2:
        self.plot_points(data)

      #group sorting
      cnt=data.shape[0]
      idx=np.ndarray(cnt,dtype=np.int32)
      grp_cnt=int(np.sqrt(cnt))
      grp_sz=int(np.ceil(float(cnt)/grp_cnt))
      for i in range(grp_cnt):
        a=i*grp_sz
        #print a,a+grp_sz
        if i%2:
          idx[a:a+grp_sz]=a+data[a:a+grp_sz,0].argsort()[::-1]
        else:
          idx[a:a+grp_sz]=a+data[a:a+grp_sz,0].argsort()
        #print(idx)
      data=data[idx]

      if verb&2:
        self.plot_points(data)
      plt.show()
      self.points=data

    @staticmethod
    def onclick(event):
        print 'button=%s, x=%d, y=%d, xdata=%f, ydata=%f'%(
            event.button, event.x, event.y, event.xdata, event.ydata)
        obj=event.canvas.figure.obj

    def plot_points(self,data):
      fig=plt.figure()
      ax = fig.add_subplot(1,1,1)
      #hl=ax[0].plot(x, y, color=col)
      hl=ax.plot(data[:,0],data[:,1],'r.')
      hl=ax.plot(data[:,0],data[:,1],'y--')
      cid = fig.canvas.mpl_connect('button_press_event', self.onclick)
      fig.obj=self
      self.data=data
      self.ax=ax
      self.hl=hl


if __name__=='__main__':
  from optparse import OptionParser, IndentedHelpFormatter
  class MyFormatter(IndentedHelpFormatter):
    'helper class for formating the OptionParser'

    def __init__(self):
      IndentedHelpFormatter.__init__(self)

    def format_epilog(self, epilog):
      if epilog:
        return epilog
      else:
        return ""

  def parse_args():
    'main command line interpreter function'
    #usage: gpasciiCommunicator.py --host=PPMACZT84 myPowerBRICK.cfg
    (h, t)=os.path.split(sys.argv[0]);cmd='\n  '+(t if len(h)>3 else sys.argv[0])+' '
    exampleCmd=('--host=PPMAC1391 -m 63 --cfg gather.cfg',
                'samplePowerBrick.cfg',
                '-n stackCheck1.cfg',
                '--host=PPMACZT84 stackCheck1.cfg',
                '--host=PPMACZT84 stackCheck1.cfg -v15',
               )
    epilog=__doc__+'''
Examples:'''+''.join(map(lambda s:cmd+s, exampleCmd))+'\n '

    fmt=MyFormatter()
    parser=OptionParser(epilog=epilog, formatter=fmt)

    parser.add_option('-v', '--verbose', type="int", dest='verbose', help='verbosity bits (see below)', default=0)
    parser.add_option('-m', '--mode', type="int", dest='mode', help='mode bits (see below)', default=0)
    parser.add_option('--cfg', help='config file containing json configuration structure')

    (args, other)=parser.parse_args()
    args.other=other

    sp=ShapePath(args)
    sp.run()
#------------------ Main Code ----------------------------------
  #ssh_test()
  ret=parse_args()
  exit(ret)